diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 94d4dc6..c0fc6a8 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -14,11 +14,59 @@ RUN apt install -y wget python3-rosdep # Init and update rosdep RUN [ -f /etc/ros/rosdep/sources.list.d/20-default.list ] || rosdep init && \ - rosdep update --rosdistro=humble + rosdep update --rosdistro=${ROS_DISTRO} + +# Install ROS 2 control +RUN apt update && apt install -y \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-controller-manager \ + ros-${ROS_DISTRO}-joint-state-broadcaster + +# Install nav2 +RUN apt install -y ros-${ROS_DISTRO}-navigation2 ros-${ROS_DISTRO}-nav2-bringup ros-${ROS_DISTRO}-rmw-cyclonedds-cpp + +# Install Gazebo Classic and ROS 2 Gazebo integration +RUN apt update && apt install -y \ + ros-${ROS_DISTRO}-gazebo-ros \ + ros-${ROS_DISTRO}-gazebo-ros2-control \ + ros-${ROS_DISTRO}-joint-state-publisher \ + ros-${ROS_DISTRO}-joint-state-publisher-gui \ + ros-dev-tools \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-gazebo-plugins \ + ros-${ROS_DISTRO}-gazebo-ros-pkgs \ + ros-${ROS_DISTRO}-gazebo-dev + +# Install additional ROS2 utils +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-rqt \ + ros-${ROS_DISTRO}-rqt-common-plugins \ + ros-${ROS_DISTRO}-rqt-gui \ + ros-${ROS_DISTRO}-rqt-gui-py \ + ros-${ROS_DISTRO}-rqt-image-view \ + ros-${ROS_DISTRO}-rqt-robot-steering \ + ros-${ROS_DISTRO}-rqt-tf-tree \ + ros-${ROS_DISTRO}-tf2-tools \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-plotjuggler \ + ros-${ROS_DISTRO}-plotjuggler-ros + +# Install adafruit i2c librarries +RUN apt install -y \ + python3-pip \ + python3-setuptools \ + python3-wheel \ + python3-smbus \ + i2c-tools + +RUN pip3 install --no-cache-dir \ + RPi.GPIO \ + adafruit-blinka # Set up the workspace directory WORKDIR $COLCON_WORKSPACE/src # Add sourcing ROS setup.bash to .bashrc RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc -RUN echo "export ROS_DOMAIN_ID=91" >> ~/.bashrc + diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index 0dc2d0a..88bc0a8 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -7,10 +7,20 @@ services: COLCON_WORKSPACE: /root/ros_ws volumes: - ../:/root/ros_ws/src + - /dev:/dev + - /tmp/.X11-unix:/tmp/.X11-unix environment: - ROS_DOMAIN_ID=91 - network_mode: "host" - command: ["/bin/bash", "-c", "/root/ros_ws/src/.devcontainer/entrypoint.sh"] + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + privileged: true + devices: + - "/dev:/dev" + - /dev/bus/usb:/dev/bus/usb + - /dev/i2c-0:/dev/i2c-0 + - /dev/i2c-1:/dev/i2c-1 + tty: true + command: [ "/bin/bash", "-c", "/root/ros_ws/src/.devcontainer/entrypoint.sh" ] scorpion-ros2-test: build: @@ -23,4 +33,4 @@ services: environment: - ROS_DOMAIN_ID=91 network_mode: "host" - command: ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash && cd /root/ros_ws && colcon test"] + command: [ "/bin/bash", "-c", "source /opt/ros/humble/setup.bash && cd /root/ros_ws && colcon test" ] diff --git a/README.md b/README.md index df017a1..eb2a2ec 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ [![Powered by Dev Containers](https://img.shields.io/badge/Powered%20by-Dev%20Containers-blue?logo=devcontainers&logoColor=white)](https://containers.dev/) -# Scorpion +# Pentapod This is a ROS2 software for the following older project [youtube video available here.](https://youtu.be/kcvJR5mcb1o?si=lxt_06UO4189CPcX) @@ -21,9 +21,9 @@ cd ~/my_ws/src Clone the repo ``` -git clone git@github.com:Modi1987/scorpion.git +git clone git@github.com:Modi1987/pentapod.git -cd scorpion +cd pentapod git checkout i2c_actuators ``` @@ -31,7 +31,7 @@ git checkout i2c_actuators There are bash scripts that will allow you to build your workspace you can do this on your PC if you want to run RVIZ simulations, or on Raspberry-pi4 if you want to run on real-robot ``` -cd ~/my_ws/src/scorpion/scripts +cd ~/my_ws/src/pentapod/scripts ./setup_penta_workspace.sh ``` @@ -41,7 +41,7 @@ cd ~/my_ws/src/scorpion/scripts besides to the previous steps to setup your workspace, to control the real-robot you will need to configure the i2c bus on Raspberry-pi 4, to do so on the Raspberry-pi 4: ``` -cd ~/my_ws/src/scorpion/scripts +cd ~/my_ws/src/pentapod/scripts ./setup_i2c_on_raspberry_pi4.sh ``` @@ -64,22 +64,30 @@ Finally, you can move the robot around from external PC using keyboard ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` +## If you are using Hiwonder motors over serial bus + +''' +ros2 launch penta_pod realhardware_bringup.launch.py motors_interface:=hiwonder +''' + ## Setup as a service (automatic on boot) To bring-up the robot automatically each time you boot the robot (without having to launch the brinup manually), run the following script on the Raspberry Pi ``` -cd ~/my_ws/src/scorpion/scripts +cd ~/my_ws/src/pentapod/scripts ./setup_service_penta_bringup_on_boot.sh ``` ## Run in simulation +### Using Rviz + You can run the simulation using the command ``` -ros2 launch penta_pod penta_simn_rviz.launch.py +ros2 launch penta_pod penta_sim_rviz.launch.py ``` You can move the robot around using @@ -88,6 +96,13 @@ You can move the robot around using ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` +### Gazebo simulation + +You can also run a gazebo simulation using + +``` +ros2 launch penta_gazebo_sim penta_sim_full_gazebo.launch.py +``` ## Using devcontainers @@ -131,4 +146,66 @@ cd .. source install/setup.bash ros2 launch penta_pod penta_pod.launch.py -``` \ No newline at end of file +``` + + +# Swarm of robots on same network + +You can create a swarm of the real robot, all of them connected to eachothers sharing the same network, each robot will be distnguished by its own namespace as the following: + +For 'robot1' + +``` +ros2 launch penta_pod realhardware_bringup.launch.py name_space:=robot1 mode:=real + +``` + +For 'robot2' + +``` +ros2 launch penta_pod realhardware_bringup.launch.py name_space:=robot2 mode:=real + +``` + +You can check that the topics are in the namespace, same for services and actions, you can use rqt introspection to show the ndoes graph. + +``` +~/ros_ws# ros2 topic list +/parameter_events +/robot1/actuator_setpoint_degree +/robot1/cmd_vel +/robot1/joint_setpoints +/robot1/joy +/robot1/joy/set_feedback +/robot1/limb0/joint_setpoints +/robot1/limb0/xyz_msg +/robot1/limb1/joint_setpoints +/robot1/limb1/xyz_msg +/robot1/limb2/joint_setpoints +/robot1/limb2/xyz_msg +/robot1/limb3/joint_setpoints +/robot1/limb3/xyz_msg +/robot1/limb4/joint_setpoints +/robot1/limb4/xyz_msg +/robot1/null_space_pose +/robot2/actuator_setpoint_degree +/robot2/cmd_vel +/robot2/joint_setpoints +/robot2/joy +/robot2/joy/set_feedback +/robot2/limb0/joint_setpoints +/robot2/limb0/xyz_msg +/robot2/limb1/joint_setpoints +/robot2/limb1/xyz_msg +/robot2/limb2/joint_setpoints +/robot2/limb2/xyz_msg +/robot2/limb3/joint_setpoints +/robot2/limb3/xyz_msg +/robot2/limb4/joint_setpoints +/robot2/limb4/xyz_msg +/robot2/null_space_pose +/rosout +/tf +``` + +You can publish on `/robot1/cmd_vel` to control first robot, and on `robot2/cmd_vel` to control second robot and so forth. If you do not have real robot you can use `mode:=virtual` and test \ No newline at end of file diff --git a/base_twerk/include/base_twerk/base_twerk_action_server.hpp b/base_twerk/include/base_twerk/base_twerk_action_server.hpp index 98c486c..a19d68b 100644 --- a/base_twerk/include/base_twerk/base_twerk_action_server.hpp +++ b/base_twerk/include/base_twerk/base_twerk_action_server.hpp @@ -44,6 +44,8 @@ class BaseTwerkActionServer { auto execute(const std::shared_ptr goal_handle) -> void; + auto is_target_valid(const std::shared_ptr goal_handle) -> bool; + auto declare_parameters() -> void; auto get_parameters() -> void; @@ -52,11 +54,16 @@ class BaseTwerkActionServer { auto calculate_twerk_pose_from_goal( rclcpp::Time start_time, PoseStamped start_pose, const std::shared_ptr goal_handle) -> PoseStamped; - + auto quiry_current_base_pose() -> std::optional; - double update_interval_millis_double_; - double max_permissible_displacement_meter_; + struct TwerkYamlConfigs { + double update_interval_millis_double_; + double max_permissible_displacement_meter_; + double min_permissible_displacement_meter_; + double max_permissible_rotation_rad_; + double min_permissible_rotation_rad_; + } twerk_yaml_configs_; struct ReceviedPoseStamped { rclcpp::Time timestamp; diff --git a/base_twerk/include/base_twerk/base_twerk_helper_funs.hpp b/base_twerk/include/base_twerk/base_twerk_helper_funs.hpp new file mode 100644 index 0000000..85d7edc --- /dev/null +++ b/base_twerk/include/base_twerk/base_twerk_helper_funs.hpp @@ -0,0 +1,25 @@ +#ifndef BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_ +#define BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_ + +#include "math.h" + +auto get_rounded_dance_time_from_goal_millis(double w, int dance_time_millis) + -> std::chrono::milliseconds { + + constexpr double to_millis = 1000.0; + + auto dance_period_seconds = 2 * M_PI / w; + auto dance_time_seconds = static_cast(dance_time_millis) / to_millis; + + auto num_periods = dance_time_seconds / dance_period_seconds; + + auto rounded_periods = + std::round(num_periods) + 1; // +1 to ensure at least one period + + auto rounded_dance_time_millis = + static_cast(rounded_periods * dance_period_seconds * to_millis); + + return std::chrono::milliseconds(rounded_dance_time_millis); +} + +#endif // BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_ \ No newline at end of file diff --git a/base_twerk/launch/base_twerk_action_server.launch.py b/base_twerk/launch/base_twerk_action_server.launch.py index 90081e3..c09aabe 100644 --- a/base_twerk/launch/base_twerk_action_server.launch.py +++ b/base_twerk/launch/base_twerk_action_server.launch.py @@ -2,21 +2,41 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ config = os.path.join( get_package_share_directory("penta_description"), "config", "general_config.yaml", ) - ld = LaunchDescription() ld.add_action( Node( package="base_twerk", executable="base_twerk_action_server_node", output="screen", + namespace=LaunchConfiguration("name_space"), parameters=[config], + remappings=[ + # services + ("cmd_null_setpoint", "base_twerk/cmd_null_setpoint"), + ("get_current_null_pose", "base_twerk/get_current_null_pose"), + # topics + ], ) ) return ld diff --git a/base_twerk/launch/null_pose_publisher.launch.py b/base_twerk/launch/null_pose_publisher.launch.py index 8de29ef..62588bd 100644 --- a/base_twerk/launch/null_pose_publisher.launch.py +++ b/base_twerk/launch/null_pose_publisher.launch.py @@ -2,21 +2,42 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ config = os.path.join( get_package_share_directory("penta_description"), "config", "general_config.yaml", ) - ld = LaunchDescription() ld.add_action( Node( package="base_twerk", executable="base_twerk_publisher_node", output="screen", + namespace=LaunchConfiguration("name_space"), parameters=[config], + remappings=[ + # services + ("cmd_null_setpoint", "base_twerk/cmd_null_setpoint"), + ("get_current_null_pose", "base_twerk/get_current_null_pose"), + # topics + ("null_space_pose", "null_space_pose") + ], ) ) return ld diff --git a/base_twerk/src/base_twerk/base_twerk_action_server.cpp b/base_twerk/src/base_twerk/base_twerk_action_server.cpp index 49a958d..1b448e6 100644 --- a/base_twerk/src/base_twerk/base_twerk_action_server.cpp +++ b/base_twerk/src/base_twerk/base_twerk_action_server.cpp @@ -1,4 +1,5 @@ #include "base_twerk/base_twerk_action_server.hpp" +#include "base_twerk/base_twerk_helper_funs.hpp" #include "commons/quaternion_utils.hpp" namespace penta_pod::kin::base_twerk { @@ -61,23 +62,15 @@ auto BaseTwerkActionServer::execute( const auto goal = goal_handle->get_goal(); auto result = std::make_shared(); - double mag_displacement = - std::sqrt(goal->r[0] * goal->r[0] + goal->r[1] * goal->r[1] + - goal->r[2] * goal->r[2]); - if (mag_displacement > max_permissible_displacement_meter_) { - auto message = "Action aborted for the specified displacement is: " + - std::to_string(mag_displacement) + - " [meter], which is bigger than the permissible value " - "specified in yaml as " + - std::to_string(max_permissible_displacement_meter_); - result->result_message = message; - goal_handle->abort(result); - RCLCPP_ERROR(node_->get_logger(), message.c_str()); + // check if target is valid + if (!is_target_valid(goal_handle)) { + RCLCPP_ERROR(node_->get_logger(), "Invalid target, action aborted"); return; } const auto millis_in_a_second = 1000.0; - rclcpp::Rate rate(millis_in_a_second / update_interval_millis_double_); + rclcpp::Rate rate(millis_in_a_second / + twerk_yaml_configs_.update_interval_millis_double_); auto is_timed_out = [this](rclcpp::Time start_time, @@ -113,7 +106,7 @@ auto BaseTwerkActionServer::execute( } auto dance_time_millis = - std::chrono::milliseconds(static_cast(goal->dance_time_millis)); + get_rounded_dance_time_from_goal_millis(goal->w, goal->dance_time_millis); // start control loop for base pose motion auto start_time = node_->now(); @@ -223,7 +216,7 @@ auto BaseTwerkActionServer::calculate_twerk_pose_from_goal( auto roll = rpy[0]; auto pitch = rpy[1]; auto yaw = rpy[2]; - pose.pose.orientation = rpy_to_quaternion(yaw, pitch, roll); + pose.pose.orientation = penta_pod::kin::commons::quaternion_utils::rpy_to_quaternion(yaw, pitch, roll); return pose; } @@ -246,10 +239,83 @@ auto BaseTwerkActionServer::call_setpoint_client( return true; } +auto BaseTwerkActionServer::is_target_valid( + const std::shared_ptr goal_handle) -> bool { + // Check goal handle validity + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal handle is null"); + return false; + } + + auto result = std::make_shared(); + + const auto goal = goal_handle->get_goal(); + + // Check input fields + if (goal->w == 0.0) { + auto message = "Action target goal for the specified frequency is: " + + std::to_string(goal->w) + + " [Hz], which is not valid. It should not be 0."; + result->result_message = message; + goal_handle->abort(result); + RCLCPP_ERROR(node_->get_logger(), message.c_str()); + return false; + } + + if (goal->dance_time_millis <= 0) { + auto message = + "Action target goal for the specified dance time is: " + + std::to_string(goal->dance_time_millis) + + " [milliseconds], which is not valid. It should be positive."; + result->result_message = message; + goal_handle->abort(result); + RCLCPP_ERROR(node_->get_logger(), message.c_str()); + return false; + } + + double mag_displacement = + std::sqrt(goal->r[0] * goal->r[0] + goal->r[1] * goal->r[1] + + goal->r[2] * goal->r[2]); + double comparision_value = + twerk_yaml_configs_.max_permissible_displacement_meter_; + if (mag_displacement > comparision_value) { + auto message = "The specified displacement magnitude is: " + + std::to_string(mag_displacement) + + " [meter], which is bigger than the permissible value " + "specified in yaml as " + + std::to_string(comparision_value); + result->result_message = message; + goal_handle->abort(result); + RCLCPP_ERROR(node_->get_logger(), message.c_str()); + return false; + } + + double mag_rotation = + std::sqrt(goal->r[3] * goal->r[3] + goal->r[4] * goal->r[4] + + goal->r[5] * goal->r[5]); + comparision_value = twerk_yaml_configs_.max_permissible_rotation_rad_; + if (mag_rotation > comparision_value) { + auto message = + "The specified rotation magnitude is: " + std::to_string(mag_rotation) + + " [rad], which is bigger than the permissible value " + "specified in yaml as " + + std::to_string(comparision_value); + result->result_message = message; + goal_handle->abort(result); + RCLCPP_ERROR(node_->get_logger(), message.c_str()); + return false; + } + + return true; +} + auto BaseTwerkActionServer::declare_parameters() -> void { node_->declare_parameter( "base_twerk.action_server_service_call_interval_millis"); node_->declare_parameter("base_twerk.max_permissible_displacement"); + node_->declare_parameter("base_twerk.max_permissible_rotation"); + node_->declare_parameter("base_twerk.min_permissible_displacement"); + node_->declare_parameter("base_twerk.min_permissible_rotation"); } auto BaseTwerkActionServer::get_parameters() -> void { @@ -260,24 +326,45 @@ auto BaseTwerkActionServer::get_parameters() -> void { throw std::runtime_error(error_message); } }; - int update_interval_millis_int_; + int update_interval_millis_int; load_param("base_twerk.action_server_service_call_interval_millis", - update_interval_millis_int_, + update_interval_millis_int, "No parameter " "base_twerk.action_server_service_call_interval_millis is found."); RCLCPP_INFO(node_->get_logger(), " base_twerk.action_server_service_call_interval_millis is %d " "[milliseconds]", - update_interval_millis_int_); - update_interval_millis_double_ = - static_cast(update_interval_millis_int_); + update_interval_millis_int); + twerk_yaml_configs_.update_interval_millis_double_ = + static_cast(update_interval_millis_int); load_param("base_twerk.max_permissible_displacement", - max_permissible_displacement_meter_, + twerk_yaml_configs_.max_permissible_displacement_meter_, "No parameter base_twerk.max_permissible_displacement is found."); RCLCPP_INFO(node_->get_logger(), " base_twerk.max_permissible_displacement is %f [m]", - max_permissible_displacement_meter_); + twerk_yaml_configs_.max_permissible_displacement_meter_); + + load_param("base_twerk.max_permissible_rotation", + twerk_yaml_configs_.max_permissible_rotation_rad_, + "No parameter base_twerk.max_permissible_rotation is found."); + RCLCPP_INFO(node_->get_logger(), + " base_twerk.max_permissible_rotation is %f [rad]", + twerk_yaml_configs_.max_permissible_rotation_rad_); + + load_param("base_twerk.min_permissible_displacement", + twerk_yaml_configs_.min_permissible_displacement_meter_, + "No parameter base_twerk.min_permissible_displacement is found."); + RCLCPP_INFO(node_->get_logger(), + " base_twerk.min_permissible_displacement is %f [m]", + twerk_yaml_configs_.min_permissible_displacement_meter_); + + load_param("base_twerk.min_permissible_rotation", + twerk_yaml_configs_.min_permissible_rotation_rad_, + "No parameter base_twerk.min_permissible_rotation is found."); + RCLCPP_INFO(node_->get_logger(), + " base_twerk.min_permissible_rotation is %f [rad]", + twerk_yaml_configs_.min_permissible_rotation_rad_); } } // namespace penta_pod::kin::base_twerk diff --git a/commons/include/commons/quaternion_utils.hpp b/commons/include/commons/quaternion_utils.hpp index e7b6cb6..a82bf87 100644 --- a/commons/include/commons/quaternion_utils.hpp +++ b/commons/include/commons/quaternion_utils.hpp @@ -3,6 +3,8 @@ #include "geometry_msgs/msg/quaternion.hpp" +namespace penta_pod::kin::commons::quaternion_utils { + using geometry_msgs::msg::Quaternion; Quaternion rpy_to_quaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X) @@ -35,4 +37,6 @@ Quaternion hamilton_product(Quaternion u, Quaternion v) } +} // namespace penta_pod::kin::commons::quaternion_utils + #endif // COMMONS_QUAT_UTILS_HPP_ \ No newline at end of file diff --git a/gait_generator/include/gait_generator/gait_generator.hpp b/gait_generator/include/gait_generator/gait_generator.hpp index a3638c0..d9040b7 100644 --- a/gait_generator/include/gait_generator/gait_generator.hpp +++ b/gait_generator/include/gait_generator/gait_generator.hpp @@ -36,10 +36,17 @@ class GaitGenerator { rclcpp::Node::SharedPtr node_; std::vector q_state; - double current_phase_, max_gait_linear_speed_, max_gait_turning_speed_; + double current_phase_; bool is_walking_{false}; int feet_num_; + struct GaitParameters { + double gait_radial_frequency; // rad/s + double step_height; // m + double max_gait_linear_speed; // m/s + double max_gait_turning_speed; // rad/s + } gait_parameters_; + struct GatiPatterns { int active_gait_index_ = 0; int gaits_num_ = 0; diff --git a/gait_generator/launch/gait_generator.launch.py b/gait_generator/launch/gait_generator.launch.py index dfece2e..7978d85 100644 --- a/gait_generator/launch/gait_generator.launch.py +++ b/gait_generator/launch/gait_generator.launch.py @@ -2,9 +2,23 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ config = os.path.join( get_package_share_directory("penta_description"), "config", @@ -17,7 +31,13 @@ def generate_launch_description(): executable="gait_generator_node", output="screen", parameters=[config], - ) + namespace=LaunchConfiguration("name_space"), + remappings=[ + # consumed topics + ("cmd_vel", "cmd_vel"), + ("null_space_pose", "null_space_pose"), + ], + ), ) ld.add_action( Node( @@ -25,6 +45,11 @@ def generate_launch_description(): executable="base_tf_broadcaster_node", output="screen", parameters=[config], + namespace=LaunchConfiguration("name_space"), + remappings=[ + # output topics + ("null_space_pose", "null_space_pose"), + ], ) ) return ld diff --git a/gait_generator/src/gait_generator/gait_generator_core.cpp b/gait_generator/src/gait_generator/gait_generator_core.cpp index 1e0fd8c..de47488 100644 --- a/gait_generator/src/gait_generator/gait_generator_core.cpp +++ b/gait_generator/src/gait_generator/gait_generator_core.cpp @@ -98,13 +98,16 @@ void GaitGenerator::cmd_vel_sub_callback( auto mag = std::sqrt(msg->linear.x * msg->linear.x + msg->linear.y * msg->linear.y); - if (mag > max_gait_linear_speed_) { - msg->linear.x = msg->linear.x * max_gait_linear_speed_ / mag; - msg->linear.y = msg->linear.y * max_gait_linear_speed_ / mag; + if (mag > gait_parameters_.max_gait_linear_speed) { + msg->linear.x = + msg->linear.x * gait_parameters_.max_gait_linear_speed / mag; + msg->linear.y = + msg->linear.y * gait_parameters_.max_gait_linear_speed / mag; } mag = std::abs(msg->angular.z); - if (mag > max_gait_turning_speed_) { - msg->angular.z = msg->angular.z * max_gait_turning_speed_ / mag; + if (mag > gait_parameters_.max_gait_turning_speed) { + msg->angular.z = + msg->angular.z * gait_parameters_.max_gait_turning_speed / mag; } cmd_vel_ = *msg; /* @@ -125,7 +128,7 @@ void GaitGenerator::cmd_null_pos_sub_callback( body_basefootprint_.rotation.y = msg->pose.orientation.y; body_basefootprint_.rotation.z = msg->pose.orientation.z; body_basefootprint_.rotation.w = msg->pose.orientation.w; - + // consider the rotation absolute (initial rotation must be the identity) // body_basefootprint_.rotation. = msg->rotation; // RCLCPP_INFO(node_->get_logger(), @@ -139,7 +142,7 @@ void GaitGenerator::cmd_null_pos_sub_callback( void GaitGenerator::update_phase(double delta_t_milli) { auto delta_t_sec = delta_t_milli / 1000.; - double w = 2.5; + double w = gait_parameters_.gait_radial_frequency; // check if cmd_vel is zero and feet near the equilibrium double vel_mag = std::sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x + @@ -172,12 +175,12 @@ void GaitGenerator::update_feet_positions(double delta_t_milli) { double dx = cmd_vel_.linear.x * delta_t_sec; double dy = cmd_vel_.linear.y * delta_t_sec; double d_theta = cmd_vel_.angular.z * delta_t_sec; - double b = 0.05; + double b = gait_parameters_.step_height; auto gait_pattern = gait_patterns_.get_active_pattern(); for (int i = 0; i < feet_num_; i++) { auto foot_index = gait_pattern[i]; - auto temp = foot_pos_z_generator(b, current_phase_, - phase_shift_vec_[i], feet_num_); + auto temp = + foot_pos_z_generator(b, current_phase_, phase_shift_vec_[i], feet_num_); if (temp == 0.) { double x = feet_pos_in_footprint_[foot_index].x; double y = feet_pos_in_footprint_[foot_index].y; diff --git a/gait_generator/src/gait_generator/gait_generator_parameters.cpp b/gait_generator/src/gait_generator/gait_generator_parameters.cpp index fbbf191..7200216 100644 --- a/gait_generator/src/gait_generator/gait_generator_parameters.cpp +++ b/gait_generator/src/gait_generator/gait_generator_parameters.cpp @@ -17,6 +17,8 @@ void GaitGenerator::declare_parameters() { node_->declare_parameter("gait_parameters.gait_patterns.num"); node_->declare_parameter>( "gait_parameters.gait_patterns.feet_order", std::vector{}); + node_->declare_parameter("gait_parameters.gait_radial_frequency"); + node_->declare_parameter("gait_parameters.step_height"); } void GaitGenerator::load_parameters() { @@ -121,19 +123,34 @@ void GaitGenerator::load_parameters() { body_basefootprint_ = transform; // Load gait parameters - load_param("gait_parameters.max_gait_linear_speed", max_gait_linear_speed_, + load_param("gait_parameters.max_gait_linear_speed", + gait_parameters_.max_gait_linear_speed, "Parameter gait_parameters.max_gait_linear_speed was not found."); RCLCPP_INFO( node_->get_logger(), "Loaded gait_parameters.max_gait_linear_speed value is: %f [m/sec]", - max_gait_linear_speed_); + gait_parameters_.max_gait_linear_speed); - load_param("gait_parameters.max_gait_turning_speed", max_gait_turning_speed_, + load_param("gait_parameters.max_gait_turning_speed", + gait_parameters_.max_gait_turning_speed, "Parameter gait_parameters.max_gait_turning_speed was not found."); RCLCPP_INFO( node_->get_logger(), "Loaded gait_parameters.max_gait_turning_speed value is: %f [rad/sec]", - max_gait_turning_speed_); + gait_parameters_.max_gait_turning_speed); + + load_param("gait_parameters.gait_radial_frequency", + gait_parameters_.gait_radial_frequency, + "Parameter gait_parameters.gait_radial_frequency was not found."); + RCLCPP_INFO(node_->get_logger(), + "Loaded gait_parameters.gait_radial_frequency value is: %f [Hz]", + gait_parameters_.gait_radial_frequency); + + load_param("gait_parameters.step_height", gait_parameters_.step_height, + "Parameter gait_parameters.step_height was not found."); + RCLCPP_INFO(node_->get_logger(), + "Loaded gait_parameters.step_height value is: %f [m]", + gait_parameters_.step_height); // load gait patterns load_param("gait_parameters.gait_patterns.num", gait_patterns_.gaits_num_, diff --git a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp index 6cba46c..a812677 100644 --- a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp +++ b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp @@ -34,6 +34,7 @@ class JointsAggregator { int limbs_num_; std::vector joints_per_limb_; int update_interval_millis_; + int number_of_messages_forcely_published_on_startup_ {0}; int publish_on_startup_counter_ {0}; bool publish_joints_only_on_value_change_{false}; diff --git a/joints_aggregator/launch/joints_aggregator.launch.py b/joints_aggregator/launch/joints_aggregator.launch.py index e07e58e..0467f14 100644 --- a/joints_aggregator/launch/joints_aggregator.launch.py +++ b/joints_aggregator/launch/joints_aggregator.launch.py @@ -2,9 +2,22 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription import launch_ros.actions - +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ config = os.path.join( get_package_share_directory("penta_description"), "config", @@ -16,7 +29,11 @@ def generate_launch_description(): package="joints_aggregator", executable="joints_aggregator_node", output="screen", + namespace=LaunchConfiguration("name_space"), parameters=[config], + remappings=[ + ("joint_setpoints", "joint_setpoints"), + ], ), ] ) diff --git a/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp b/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp index 415fc75..b8a5e7d 100644 --- a/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp +++ b/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp @@ -36,10 +36,11 @@ JointsAggregator::JointsAggregator() this->previous_q_ = std::vector(joints_count, 0.); joint_state_publisher_ = - node_->create_publisher("joint_states", 10); + node_->create_publisher("joint_setpoints", + 10); for (int i = 0; i < limbs_num_; i++) { - std::string topic_string = "limb" + std::to_string(i) + "/joint_state"; + std::string topic_string = "limb" + std::to_string(i) + "/joint_setpoints"; limb_joints_subscriber_.push_back( node_->create_subscription( topic_string, 10, @@ -65,8 +66,8 @@ JointsAggregator::JointsAggregator() // publish logic auto force_publish_on_startup = [this]() -> bool { - constexpr long times_to_publish_on_startup = 1000; - if (publish_on_startup_counter_ < times_to_publish_on_startup) { + if (publish_on_startup_counter_ < + number_of_messages_forcely_published_on_startup_) { publish_on_startup_counter_++; return true; } @@ -108,6 +109,10 @@ void JointsAggregator::declare_parameters() { // publish only on value change node_->declare_parameter( "joints_aggregator.publish_joints_only_on_value_change"); + // number of messages forcely published on startup + node_->declare_parameter( + "joints_aggregator.number_of_messages_forcely_published_on_startup", + 0); } auto JointsAggregator::get_parameters() -> bool { @@ -175,14 +180,30 @@ auto JointsAggregator::get_parameters() -> bool { publish_joints_only_on_value_change_)) { RCLCPP_INFO_STREAM( node_->get_logger(), - "loaded joints_aggregator.publish_joints_only_on_value_change) is: " + "loaded joints_aggregator.publish_joints_only_on_value_change: " << publish_joints_only_on_value_change_); } else { RCLCPP_ERROR( node_->get_logger(), "ERROR, can not load " "joints_aggregator.publish_joints_only_on_value_change parameter"); - rclcpp::shutdown(); + return false; + } + // load number_of_messages_forcely_published_on_startup_ + if (node_->get_parameter( + "joints_aggregator.number_of_messages_forcely_published_on_startup", + number_of_messages_forcely_published_on_startup_)) { + RCLCPP_INFO_STREAM( + node_->get_logger(), + "loaded " + "joints_aggregator.number_of_messages_forcely_published_on_startup: " + << number_of_messages_forcely_published_on_startup_); + } else { + RCLCPP_ERROR( + node_->get_logger(), + "ERROR, can not load " + "joints_aggregator.number_of_messages_forcely_published_on_startup " + "parameter"); return false; } diff --git a/limb_kin_chain/src/limb_kin_chain/limb_kin_chain_node.cpp b/limb_kin_chain/src/limb_kin_chain/limb_kin_chain_node.cpp index 2dd843f..6058128 100644 --- a/limb_kin_chain/src/limb_kin_chain/limb_kin_chain_node.cpp +++ b/limb_kin_chain/src/limb_kin_chain/limb_kin_chain_node.cpp @@ -96,7 +96,7 @@ LimbNode::LimbNode(Limb limb) } joint_state_publisher_ = - node_->create_publisher("joint_state", 10); + node_->create_publisher("joint_setpoints", 10); // you can publish initial joints states once sensor_msgs::msg::JointState msg; msg.name = joints_names; diff --git a/penta_description/CMakeLists.txt b/penta_description/CMakeLists.txt index 6f8c5b1..5ef0e79 100644 --- a/penta_description/CMakeLists.txt +++ b/penta_description/CMakeLists.txt @@ -13,6 +13,7 @@ install( config urdf launch + meshes DESTINATION share/${PROJECT_NAME} ) diff --git a/penta_description/README.md b/penta_description/README.md index 4cdac71..8bf1ea2 100644 --- a/penta_description/README.md +++ b/penta_description/README.md @@ -25,4 +25,30 @@ To run rviz: ``` ros2 launch penta_description penta_rviz.launch.py -``` \ No newline at end of file +``` + + +## Move the joints in gazebo simulation + + +``` +ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{ + trajectory: { + header: { + stamp: {sec: 0, nanosec: 0}, + frame_id: '' + }, + joint_names: ['limb0/joint0', 'limb0/joint2'], + points: [ + { + positions: [1.0, 0.5], + velocities: [0.0, 0.0], + time_from_start: {sec: 2, nanosec: 0} + } + ] + }, + path_tolerance: [], + goal_tolerance: [], + goal_time_tolerance: {sec: 1, nanosec: 0} +}" +``` diff --git a/penta_description/config/general_config.yaml b/penta_description/config/general_config.yaml index 0194afe..94f712c 100644 --- a/penta_description/config/general_config.yaml +++ b/penta_description/config/general_config.yaml @@ -20,8 +20,12 @@ ] init_body_basefootprint_transform: [ 0.0, 0.0, 0.158, 0.0, 0.0, 0.0, 1.0 + # 0.0, 0.0, 0.0158, 0.0, 0.0, 0.0, 1.0 ] - joint_angles_at_initial_pose_degree: [ + joint_angles_at_initial_pose_degree: [ # this is to be used by the i2c_actuators_params + # when used on real hardware, make sure thet + # init_body_basefootprint_transform is set to the correct value + # 0.0, 0.0, 0.158, 0.0, 0.0, 0.0, 1.0 0.0, 0.0, 90.0, 0.0, 0.0, 90.0, 0.0, 0.0, 90.0, @@ -29,6 +33,8 @@ 0.0, 0.0, 90.0, ] gait_parameters: + gait_radial_frequency: 2.5 # rad per sec + step_height: 0.05 # meter max_gait_linear_speed: 0.035 # meter per sec max_gait_turning_speed: 0.25 # rad per sec gait_patterns: @@ -39,12 +45,16 @@ 0, 4, 3, 2, 1 ] joints_aggregator: - joints_update_interval_millis: 10 + joints_update_interval_millis: 33 publish_joints_only_on_value_change: true + number_of_messages_forcely_published_on_startup: 1000 # 0 means no messages will be published on startup base_twerk: - base_frame_transform_update_interval_millis: 20 - action_server_service_call_interval_millis: 100 - max_permissible_displacement: 0.075 + base_frame_transform_update_interval_millis: 33 + action_server_service_call_interval_millis: 33 + max_permissible_displacement: 0.075 # meter + max_permissible_rotation: 0.15 # rad + min_permissible_displacement: 0.001 # meter + min_permissible_rotation: 0.001 # rad tracking_linear_velocity: 0.5 # [m/sec] tracking_angular_velocity: 1.0 # [rad/sec] i2c_actuators_params: @@ -83,4 +93,44 @@ 2500.0, 2500.0, 2500.0, 2500.0, 2500.0, 2500.0, 2500.0, 2500.0, 2500.0, - ] \ No newline at end of file + ] + hiwonder: + serial_port: /dev/ttyUSB0 + baudrate: 115200 + update_interval_sec: 0.1 + servo_actuation_range_degree: [ # servo angle range + 270.0, 270.0, 270.0, + 270.0, 270.0, 270.0, + 270.0, 270.0, 270.0, + 270.0, 270.0, 270.0, + 270.0, 270.0, 270.0, + ] + servo_min_ticks: [ # Hiwnoder servo min ticks (zero) + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + ] + servo_max_ticks: [ # Hiwonder, servo max ticks (motor type) + 1000, 1000, 1000, + 1000, 1000, 1000, + 1000, 1000, 1000, + 1000, 1000, 1000, + 1000, 1000, 1000, + ] + actuator_angle_bias_at_joint_zero_degree: [ # considering the motors are at their midrange at 135 degrees when robot joint is at zero" + 135.0, 135.0, 135.0, + 135.0, 135.0, 135.0, + 135.0, 135.0, 135.0, + 135.0, 135.0, 135.0, + 135.0, 135.0, 135.0, + ] + dir: [ + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, + ] + actuation_time_ratio: 0.8 diff --git a/penta_description/config/ros2_control_params.yaml b/penta_description/config/ros2_control_params.yaml new file mode 100644 index 0000000..90193be --- /dev/null +++ b/penta_description/config/ros2_control_params.yaml @@ -0,0 +1,66 @@ +controller_manager: + ros__parameters: + update_rate: 100 + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +joint_trajectory_controller: + ros__parameters: + joints: + - limb0/joint0 + - limb0/joint1 + - limb0/joint2 + - limb1/joint0 + - limb1/joint1 + - limb1/joint2 + - limb2/joint0 + - limb2/joint1 + - limb2/joint2 + - limb3/joint0 + - limb3/joint1 + - limb3/joint2 + - limb4/joint0 + - limb4/joint1 + - limb4/joint2 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + allow_partial_joints_goal: true + # open_loop_control: true + # constraints: + # goal_time: 2.0 + # stopped_velocity_tolerance: 0.01 + # leg1/joint0: + # trajectory: 0.2 + # goal: 0.1 + # ... Add constraints for each joint as needed + +forward_position_controller: + ros__parameters: + joints: + - limb0/joint0 + - limb0/joint1 + - limb0/joint2 + - limb1/joint0 + - limb1/joint1 + - limb1/joint2 + - limb2/joint0 + - limb2/joint1 + - limb2/joint2 + - limb3/joint0 + - limb3/joint1 + - limb3/joint2 + - limb4/joint0 + - limb4/joint1 + - limb4/joint2 + interface_name: position \ No newline at end of file diff --git a/penta_description/launch/penta_rviz.launch.py b/penta_description/launch/penta_rviz.launch.py index 6b8185e..d63c3b1 100644 --- a/penta_description/launch/penta_rviz.launch.py +++ b/penta_description/launch/penta_rviz.launch.py @@ -11,6 +11,14 @@ # This is a launch rviz2 on a laptop for the toperware_bot def generate_launch_description(): + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + + """ Nodes """ rviz_config_file = PathJoinSubstitution( [FindPackageShare('penta_description'), 'config', 'penta.rviz']) urdf_path = os.path.join( @@ -23,6 +31,7 @@ def generate_launch_description(): robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), value_type=str) return LaunchDescription([ + name_space_arg, DeclareLaunchArgument( 'config_file', default_value=rviz_config_file, @@ -31,6 +40,7 @@ def generate_launch_description(): Node( package='rviz2', executable='rviz2', + namespace=LaunchConfiguration('name_space'), name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('config_file')] @@ -39,6 +49,7 @@ def generate_launch_description(): Node( package='robot_state_publisher', executable='robot_state_publisher', + namespace=LaunchConfiguration('name_space'), name='robot_state_publisher', output='screen', parameters=[ diff --git a/penta_description/launch/robot_state_publisher.launch.py b/penta_description/launch/robot_state_publisher.launch.py new file mode 100644 index 0000000..9b62822 --- /dev/null +++ b/penta_description/launch/robot_state_publisher.launch.py @@ -0,0 +1,63 @@ +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.parameter_descriptions import ParameterValue +from launch.substitutions import Command, LaunchConfiguration +from launch import LaunchDescription +import os +import subprocess + +def load_penta_pod_urdf(): + pkg_share = get_package_share_directory('penta_description') + xacro_path = os.path.join( + pkg_share, + 'urdf', + 'penta.urdf.xacro' + ) + xacro_cmd = ['xacro', xacro_path] + completed_process = subprocess.run(xacro_cmd, text=True, capture_output=True) + if completed_process.returncode != 0: + raise RuntimeError(f"Command '{' '.join(xacro_cmd)}' failed with error code {completed_process.returncode}") + urdf_text = completed_process.stdout + hard_path = f"file://{pkg_share}" + urdf_remove_relatvie_path = urdf_text.replace('package://penta_description', hard_path) + return urdf_remove_relatvie_path + + +# This is a launch rviz2 on a laptop for the toperware_bot +def generate_launch_description(): + robot_description = load_penta_pod_urdf() + print("robot_description: ", robot_description) + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + remapping_arg = DeclareLaunchArgument( + 'joint_states_remappings', + default_value='/joint_states', + description='Robot state publisher input topic' + ) + """ Lauch description """ + return LaunchDescription([ + name_space_arg, + remapping_arg, + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace=LaunchConfiguration('name_space'), + name='robot_state_publisher', + output='screen', + parameters=[ + {'robot_description': robot_description} + ], + remappings=[ + ('joint_states', LaunchConfiguration('joint_states_remappings')), + ] + ), + ]) \ No newline at end of file diff --git a/penta_description/meshes/visual/penta_stl_v1/Chassi spacers.stl b/penta_description/meshes/visual/penta_stl_v1/Chassi spacers.stl new file mode 100644 index 0000000..5ccdbda Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Chassi spacers.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Chassi_bottom.stl b/penta_description/meshes/visual/penta_stl_v1/Chassi_bottom.stl new file mode 100644 index 0000000..e7137d3 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Chassi_bottom.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Chassi_middle.stl b/penta_description/meshes/visual/penta_stl_v1/Chassi_middle.stl new file mode 100644 index 0000000..0d0517f Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Chassi_middle.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Chassi_top.stl b/penta_description/meshes/visual/penta_stl_v1/Chassi_top.stl new file mode 100644 index 0000000..a09f058 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Chassi_top.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Chassi_top_cover.stl b/penta_description/meshes/visual/penta_stl_v1/Chassi_top_cover.stl new file mode 100644 index 0000000..f3c8210 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Chassi_top_cover.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Lower_arm.stl b/penta_description/meshes/visual/penta_stl_v1/Lower_arm.stl new file mode 100644 index 0000000..74029d4 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Lower_arm.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Resting supports.stl b/penta_description/meshes/visual/penta_stl_v1/Resting supports.stl new file mode 100644 index 0000000..3b1560c Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Resting supports.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Shoe.stl b/penta_description/meshes/visual/penta_stl_v1/Shoe.stl new file mode 100644 index 0000000..705edff Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Shoe.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Shoulder.stl b/penta_description/meshes/visual/penta_stl_v1/Shoulder.stl new file mode 100644 index 0000000..a5d78d5 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Shoulder.stl differ diff --git a/penta_description/meshes/visual/penta_stl_v1/Upper_arm.stl b/penta_description/meshes/visual/penta_stl_v1/Upper_arm.stl new file mode 100644 index 0000000..bf2d365 Binary files /dev/null and b/penta_description/meshes/visual/penta_stl_v1/Upper_arm.stl differ diff --git a/penta_description/urdf/penta.urdf.xacro b/penta_description/urdf/penta.urdf.xacro index ce2c466..6e5e65c 100644 --- a/penta_description/urdf/penta.urdf.xacro +++ b/penta_description/urdf/penta.urdf.xacro @@ -1,19 +1,49 @@ + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + - + @@ -22,11 +52,9 @@ - + - - - + @@ -35,79 +63,38 @@ - - - - - - - - - - - + + - + - + + - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - - - - - - - - + + + + + - - - - - - + + + + + - + + - - - - - + + + $(find penta_description)/config/ros2_control_params.yaml + + diff --git a/penta_description/urdf/single_limb.urdf.xacro b/penta_description/urdf/single_limb.urdf.xacro index 5eada8a..ec8c39a 100644 --- a/penta_description/urdf/single_limb.urdf.xacro +++ b/penta_description/urdf/single_limb.urdf.xacro @@ -1,94 +1,208 @@ - + - - - - - - - - - - - - - - - - - - + + + + + + + - + - + + + + + + + - + - + + + + + + + - + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + 2.0 + 2.0 + 1 0 0 + + + + 2.0 + + + + + 100000.0 + 100.0 + 0.001 + 0.1 + + + 0.0 + 0.2 + 0.0 + 1.0 + + + + - + - + - + - + - + - + - + - - - + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + diff --git a/penta_description/urdf/wire_frame.urdf.xacro b/penta_description/urdf/wire_frame.urdf.xacro new file mode 100644 index 0000000..ce2c466 --- /dev/null +++ b/penta_description/urdf/wire_frame.urdf.xacro @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/penta_gazebo_sim/penta_gazebo_sim/CMakeLists.txt b/penta_gazebo_sim/penta_gazebo_sim/CMakeLists.txt new file mode 100644 index 0000000..004ea5c --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_sim/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(penta_gazebo_sim) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(gazebo_ros REQUIRED) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/penta_gazebo_sim/penta_gazebo_sim/launch/penta_gazebo.launch.py b/penta_gazebo_sim/penta_gazebo_sim/launch/penta_gazebo.launch.py new file mode 100644 index 0000000..f283112 --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_sim/launch/penta_gazebo.launch.py @@ -0,0 +1,111 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + # Paths + penta_pkg = get_package_share_directory("penta_pod") + gazebo_ros_pkg = get_package_share_directory("gazebo_ros") + + # RViz + penta_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(penta_pkg, "launch", "penta_rviz.launch.py") + ), + launch_arguments={ + 'joint_states_remappings': "/joint_states", + }.items() + ) + + # Gazebo + gazebo_ros = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(gazebo_ros_pkg, "launch", "gazebo.launch.py") + ) + ) + + # Spawn robot into Gazebo + spawn_robot = Node( + package="gazebo_ros", + executable="spawn_entity.py", + name="spawn_robot", + output="screen", + arguments=["-entity", "penta_pod", "-topic", "/robot_description"], + ) + + # ros2_control + load_joint_state_controller = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + + # load_joint_trajectory_controller = ExecuteProcess( + # cmd=[ + # "ros2", + # "control", + # "load_controller", + # "--set-state", + # "active", + # "joint_trajectory_controller", + # ], + # output="screen", + # ) + + # load_forward_position_controller = ExecuteProcess( + # cmd=[ + # "ros2", + # "run", + # "controller_manager", + # "--set-state", + # "active", + # "forward_position_controller", + # ], + # output="screen", + # ) + + penta_description_pkg = get_package_share_directory("penta_description") + + forward_position_controller_spwaner_node = Node( + package="controller_manager", + executable="spawner", + name="forward_position_controller_spawner", + output="screen", + arguments=["forward_position_controller", "--param-file", os.path.join(penta_description_pkg, "config", "ros2_control_params.yaml")], + ) + + + return LaunchDescription( + [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[forward_position_controller_spwaner_node], + ) + ), + penta_rviz_launch, + gazebo_ros, + spawn_robot, + ] + ) diff --git a/penta_gazebo_sim/penta_gazebo_sim/launch/penta_sim_full_gazebo.launch.py b/penta_gazebo_sim/penta_gazebo_sim/launch/penta_sim_full_gazebo.launch.py new file mode 100644 index 0000000..47a9f8f --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_sim/launch/penta_sim_full_gazebo.launch.py @@ -0,0 +1,147 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, + DeclareLaunchArgument, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit +from ament_index_python.packages import get_package_share_directory +import os +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + # Launch rviz sim full + penta_rviz_sim_full = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("penta_pod"), + "launch", + "penta_sim_full_rviz.launch.py", + ) + ), + launch_arguments={ + "name_space": LaunchConfiguration("name_space"), + }.items(), + ) + ld.add_action(penta_rviz_sim_full) + + # Paths + gazebo_ros_pkg = get_package_share_directory("gazebo_ros") + + # Gazebo world path + world_pkg = get_package_share_directory("penta_gazebo_world") + gazebo_world_path = os.path.join( + world_pkg, "worlds", "penta_gazebo_world.world" + ) + print(f"Gazebo world path: {gazebo_world_path}") + + # Gazebo + gazebo_ros = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(gazebo_ros_pkg, "launch", "gazebo.launch.py") + ), + launch_arguments={ + 'world': gazebo_world_path + }.items() + ) + ld.add_action(gazebo_ros) + + # Spawn robot into Gazebo + print("ToDo: when using several robots, add name_space to the robot description") + spawn_robot = Node( + package="gazebo_ros", + executable="spawn_entity.py", + name="spawn_robot", + output="screen", + arguments=[ + "-entity", + "penta_pod", + "-topic", + "/robot_description", + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.5", + "-Y", + "0.0", + ], + ) + ld.add_action(spawn_robot) + + # ros2_control + print( + "ToDo: @Mohammad, check how to fix with the joint states published by the penta_pod core implementation" + ) + load_joint_state_broadcaster = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + joint_state_after_robot_spawned = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot, + on_exit=[load_joint_state_broadcaster], + ) + ) + ld.add_action(joint_state_after_robot_spawned) + + penta_description_pkg = get_package_share_directory("penta_description") + + forward_position_controller_spawner_node = Node( + package="controller_manager", + executable="spawner", + name="forward_position_controller_spawner", + output="screen", + arguments=[ + "forward_position_controller", + "--param-file", + os.path.join(penta_description_pkg, "config", "ros2_control_params.yaml"), + ], + ) + forward_position_controller_after_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[forward_position_controller_spawner_node], + ) + ) + ld.add_action(forward_position_controller_after_joint_state_broadcaster) + + # Include the bridge + set_point_to_forward_position_controller = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros_to_ros2_control_command_bridge"), + "launch", + "forward_joint_command_bridge.launch.py", + ) + ), + launch_arguments={ + "name_space": LaunchConfiguration("name_space"), + }.items(), + ) + ld.add_action(set_point_to_forward_position_controller) + + return ld diff --git a/penta_gazebo_sim/penta_gazebo_sim/package.xml b/penta_gazebo_sim/penta_gazebo_sim/package.xml new file mode 100644 index 0000000..56c1e90 --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_sim/package.xml @@ -0,0 +1,20 @@ + + + + penta_gazebo_sim + 0.0.0 + TODO: Package description + modi + TODO: License declaration + + ament_cmake + + gazebo_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/penta_gazebo_sim/penta_gazebo_world/CMakeLists.txt b/penta_gazebo_sim/penta_gazebo_world/CMakeLists.txt new file mode 100644 index 0000000..68fccb8 --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_world/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(penta_gazebo_world) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(gazebo_ros REQUIRED) + +install(DIRECTORY worlds + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/penta_gazebo_sim/penta_gazebo_world/package.xml b/penta_gazebo_sim/penta_gazebo_world/package.xml new file mode 100644 index 0000000..e03fc24 --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_world/package.xml @@ -0,0 +1,20 @@ + + + + penta_gazebo_world + 0.0.0 + TODO: Package description + modi + TODO: License declaration + + ament_cmake + + gazebo_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/penta_gazebo_sim/penta_gazebo_world/worlds/penta_gazebo_world.world b/penta_gazebo_sim/penta_gazebo_world/worlds/penta_gazebo_world.world new file mode 100644 index 0000000..9af613e --- /dev/null +++ b/penta_gazebo_sim/penta_gazebo_world/worlds/penta_gazebo_world.world @@ -0,0 +1,49 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + + 0.001 + 1000 + 0 0 -9.81 + + sequential_impulse + 100 + 0.001 + true + -0.01 + + + + + diff --git a/penta_hiwonder_actuators/README.md b/penta_hiwonder_actuators/README.md new file mode 100644 index 0000000..551006c --- /dev/null +++ b/penta_hiwonder_actuators/README.md @@ -0,0 +1,22 @@ +# penta_hiwonder_actuators python package + +Control the pentapod, stream setpoints of the servo actuators over serial bus to Hiwonder motors + +## setup + +Make sure to fix the permissions for the serialbus to be accessible from the user space, and note down the serial port path and baudrate + +## buidling the package + +You can build this package + +``` + colcon build --packages-select penta_hiwonder_actuators +``` + +## To use on real-robot: + +``` +ros2 launch penta_pod realhardware_bringup.launch.py motors_interface:=hiwonder +``` + diff --git a/penta_hiwonder_actuators/launch/penta_hiwonder_actuators.launch.py b/penta_hiwonder_actuators/launch/penta_hiwonder_actuators.launch.py new file mode 100644 index 0000000..2536552 --- /dev/null +++ b/penta_hiwonder_actuators/launch/penta_hiwonder_actuators.launch.py @@ -0,0 +1,67 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + # Declare the 'name_space' argument with a default value of '' + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + name_space = LaunchConfiguration('name_space') + + # Declare the launch argument 'mode' + mode_arg = DeclareLaunchArgument( + 'mode', + default_value='real', + description='Choose operation mode: real or virtual' + ) + + # Path to the config file + config = os.path.join( + get_package_share_directory('penta_description'), + 'config', + 'general_config.yaml' + ) + + # Setpoint topic arg + joints_setpoint_topic_arg = DeclareLaunchArgument( + 'joints_setpoint_topic', + default_value='joint_states', + description='Topic for joint setpoints' + ) + joints_setpoint_topic = LaunchConfiguration('joints_setpoint_topic') + + # Node definition for Penta HiWonder actuators + penta_hiwonder_actuators_node = Node( + package='penta_hiwonder_actuators', + executable='penta_hiwonder_actuators_node', + output='screen', + parameters=[config], + namespace=name_space, + # Pass 'mode' argument to the node + arguments=[LaunchConfiguration('mode')], + remappings=[ + # published + ('actuator_setpoint_degree', 'actuator_setpoint_degree'), + ('actuator_ticks', 'actuator_ticks'), + # subscribed + ('joint_states', joints_setpoint_topic) + ] + ) + + # Create a launch description and add the actions + ld = LaunchDescription([ + name_space_arg, # Add the name_space argument + mode_arg, # Add the mode argument + joints_setpoint_topic_arg, + penta_hiwonder_actuators_node # Add the node + ]) + + return ld diff --git a/penta_hiwonder_actuators/package.xml b/penta_hiwonder_actuators/package.xml new file mode 100644 index 0000000..9c6bebf --- /dev/null +++ b/penta_hiwonder_actuators/package.xml @@ -0,0 +1,20 @@ + + + + penta_hiwonder_actuators + 0.0.0 + TODO: Package description + modi + TODO: License declaration + + sensor_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/penta_hiwonder_actuators/penta_hiwonder_actuators/__init__.py b/penta_hiwonder_actuators/penta_hiwonder_actuators/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py b/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py new file mode 100644 index 0000000..25a6cb6 --- /dev/null +++ b/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py @@ -0,0 +1,306 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +import serial +import time +import math +import sys +from dataclasses import dataclass +from std_msgs.msg import Float32MultiArray + +# Serial default settings, overridden from config.yaml +DEFAULT_PORT = "/dev/ttyUSB0" +DEFAULT_BAUDRATE = 115200 +DEFAULT_UPDATE_INTERVAL = 0.1 # seconds + +# --- LX-824 Communication Constants --- +HEADER = [0x55, 0x55] + +@dataclass +class TimeStamp: + last_serial_update: float + serial_update_interval: float + serial_update_hz: float + + +class PentaHiwonderActuators(Node): + def __init__(self, mode): + super().__init__('penta_hiwonder_actuators') + if mode not in ['real', 'virtual']: + self.get_logger().error(f'Invalid mode "{mode}" specified. Defaulting to "virtual" mode.') + mode = 'virtual' + + if mode == 'real': + self.real_mode_flag = True + else: + self.real_mode_flag = False + + self.get_logger().info(f'Starting hiwonder actuators control in {mode} mode') + + # Declare and load parameters + self.declare_params() + self.load_params() + self.initialize_properties() + + if self.real_mode_flag: + # Open serial connection + is_successful = self.open_serial_connection() + if not is_successful: + self.get_logger().error('Failed to open serial port. Exiting...') + rclpy.shutdown() + sys.exit(1) + + # Publisher actuators setpoint from joint states + self.setpoint_publisher_ = self.create_publisher(JointState, 'actuator_setpoint_degree', 1) + self.ticks_publisher_ = self.create_publisher(JointState, 'actuator_ticks', 1) + self.serial_hz_publisher_ = self.create_publisher(Float32MultiArray, "actuator_update_rate_hz", 1) + sub_callback_group = MutuallyExclusiveCallbackGroup() + self.joint_states_subscriber = self.create_subscription( + JointState, + "joint_states", + lambda msg : self.on_joint_states_callback(msg), + 1, + callback_group=sub_callback_group + ) + timer_callback_group = ReentrantCallbackGroup() + self.motor_update_timer = self.create_timer(self.update_interval_sec, self.update_motors_callback, callback_group=timer_callback_group) + + def open_serial_connection(self): + try: + port = self.serial_port + baudrate = self.baudrate + self.serial = serial.Serial(port, baudrate, timeout=1) + self.get_logger().info(f'Serial port {port} opened successfully at {baudrate} baudrate.') + return True + except serial.SerialException as e: + self.get_logger().error(f'Error opening serial port {port}: {e}') + return False + + def update_motors_callback(self): + if self.real_mode_flag: + # Actuate the motors + for i in range(self.joints_count): + position_degree = self.actuator_setpoint_degree[i] + actuation_range_degree = self.servo_actuation_range_degree[i] + ticks_span = self.servo_max_ticks[i] - self.servo_min_ticks[i] + position_ticks = self.servo_min_ticks[i] + float(position_degree * ticks_span) / actuation_range_degree + position_ticks = int(position_ticks) + if (self.last_position_ticks[i] == position_ticks): + continue # No change in position, skip + self.last_position_ticks[i] = position_ticks + servo_id = i + 1 # Servo IDs start from 1 + self.move_one_servo(servo_id, position_ticks) + self.publish_ticks_message() + + def initialize_properties(self): + self.index_cache = None + self.joints_states_names = [] + self.last_position_ticks = [-1] * self.joints_count # to track last sent position + self.actuator_setpoint_degree = [0.0] * self.joints_count + self.actuator_update_stamps = [TimeStamp(-1.0, -1.0, -1.0) for i in range(self.joints_count)] + self.motor_update_hz = [0.0 for i in range(self.joints_count)] + # messages + self.motors_update_msg_hz = Float32MultiArray() + self.actuator_msg_ticks = JointState() + self.actuator_msg_setpoint = JointState() + for i in range(self.joints_count): + self.actuator_setpoint_degree[i] = self.initial_joints_bias_degree[i] + for i in range(self.limbs_num): + for j in range(self.joints_per_limb[i]): + joint_name = f'limb{i}/joint{j}' + self.joints_states_names.append(joint_name) + + def on_joint_states_callback(self, msg): + pos_list = msg.position + # Sanity checks + if pos_list is None: + self.get_logger().error('Received joints setpoint positions are None, ignoring!') + return + if len(pos_list) < self.joints_count: + self.get_logger().error( + f'Received joints setpoint positions size {len(pos_list)} is less than {self.joints_count}, ignoring!' + ) + return + # Pre-map joint names to indices for O(1) lookup + if self.index_cache is None: + self.index_cache = [] + names = msg.name + for i, name in enumerate(self.joints_states_names): + if name in names: + index = names.index(name) + self.index_cache.append(index) + else: + self.get_logger().error(f"Joint {name} not found in message.") + n = len(self.index_cache) + if n < self.joints_count: + self.get_logger().error( + f'Joints index cache size {n} is less than {self.joints_count}, clearing index cache!' + ) + self.index_cache = None + return + # Compute actuator setpoints (vectorized-like) + deg_per_rad = 180.0 / math.pi + for i in range(self.joints_count): + q_rad = pos_list[self.index_cache[i]] # geometrical joint angle rads + setpoint_degree = self.dir[i] * (q_rad * deg_per_rad) + self.initial_joints_bias_degree[i] + self.actuator_setpoint_degree[i] = self.clamp( + setpoint_degree, i, 0.0, self.servo_actuation_range_degree[i] + ) + # Publish actuators setpoint + self.publish_actuators_setpoint() + + def clamp(self, value, index, min_val, max_val, margin=1.0): + min_safe_limit = min_val + margin + if value < min_safe_limit: + self.get_logger().error(f'ERROR: Servo[{index}] calculated setpoint is {value} degrees, however its minimum permissible angle is {min_safe_limit}, clamping value to {min_safe_limit}!') + return min_safe_limit + max_safe_limit = max_val - margin + if value > max_safe_limit: + self.get_logger().error(f'ERROR: Servo[{index}] calculated setpoint is {value} degrees, however maximum servo angle is {max_safe_limit}, clamping value to {max_safe_limit}!') + return max_safe_limit + return value + + def send_serial_command(self, servo_id, cmd, params=None): + """Send a packet to LX-824 servo""" + if params is None: + params = [] + length = len(params) + 3 # Length = params + CMD + checksum + length itself + packet = HEADER + [servo_id, length, cmd] + params + checksum = (~(sum(packet[2:])) & 0xFF) + packet.append(checksum) + now = time.time() + self.serial.write(bytearray(packet)) + index = servo_id - 1 + if self.actuator_update_stamps[index].last_serial_update < 0.0: + self.actuator_update_stamps[index].last_serial_update = now + else: + self.actuator_update_stamps[index].serial_update_interval = now - self.actuator_update_stamps[index].last_serial_update + self.actuator_update_stamps[index].last_serial_update = now + self.actuator_update_stamps[index].serial_update_hz = 1.0 / self.actuator_update_stamps[index].serial_update_interval + self.motor_update_hz[index] = self.actuator_update_stamps[index].serial_update_hz + time.sleep(0.005) + + def move_one_servo(self, servo_id, position_ticks): + """ + Move LX-824 servo to position (0-1000). + Default center ≈ 500. + """ + ratio = self.actuation_time_ratio + time_ms = int(self.update_interval_sec * 1000 * ratio) # time in ms + position_ticks = int(position_ticks) + pos_l = position_ticks & 0xFF + pos_h = (position_ticks >> 8) & 0xFF + time_l = time_ms & 0xFF + time_h = (time_ms >> 8) & 0xFF + self.send_serial_command(servo_id, 1, [pos_l, pos_h, time_l, time_h]) + + def publish_actuators_setpoint(self): + self.actuator_msg_setpoint.header.stamp = self.get_clock().now().to_msg() + self.actuator_msg_setpoint.name = self.joints_states_names + self.actuator_msg_setpoint.position = self.actuator_setpoint_degree + self.setpoint_publisher_.publish(self.actuator_msg_setpoint) + + def publish_ticks_message(self): + self.actuator_msg_ticks.header.stamp = self.get_clock().now().to_msg() + self.actuator_msg_ticks.name = self.joints_states_names + self.actuator_msg_ticks.position = [float(tick) for tick in self.last_position_ticks] + self.ticks_publisher_.publish(self.actuator_msg_ticks) + self.motors_update_msg_hz.data = self.motor_update_hz + self.serial_hz_publisher_.publish(self.motors_update_msg_hz) + + def declare_params(self): + # Declare robot geometry parameters + self.declare_parameter('limbs_num', 5) # Default value 5 + self.declare_parameter('joints_per_limb', [3]*5) # Default value 3 + # Hiwonder specific params + self.declare_parameter('hiwonder.serial_port', DEFAULT_PORT) + self.declare_parameter('hiwonder.baudrate', DEFAULT_BAUDRATE) + self.declare_parameter('hiwonder.update_interval_sec', DEFAULT_UPDATE_INTERVAL) + self.declare_parameter('hiwonder.actuator_angle_bias_at_joint_zero_degree', [0.0] * 15) # Default bias + self.declare_parameter('hiwonder.dir', [1.0] * 15) # Default direction (1.0 for no inversion) + self.declare_parameter('hiwonder.servo_actuation_range_degree', [270.0] * 15) # angular range degree + self.declare_parameter('hiwonder.servo_min_ticks', [0] * 15) # ticks + self.declare_parameter('hiwonder.servo_max_ticks', [1000] * 15) # ticks + self.declare_parameter('hiwonder.actuation_time_ratio', 0.8) + + def load_params(self): + # Load parameters and handle errors + self.limbs_num = self.get_parameter('limbs_num').get_parameter_value().integer_value + # Helper to format array for logging + def format_array_to_string(x_list): + return '[' + ', '.join(map(str, x_list)) + ']' + self.joints_per_limb = self.get_parameter('joints_per_limb').get_parameter_value().integer_array_value + if (len(self.joints_per_limb) != self.limbs_num): + self.get_logger().error(f" Error, limbs_num paramters {self.limbs_num} is not equal to the size of the vector joints_per_limb {len(self.joints_per_limb)}") + self.get_logger().info(f'Loaded limbs_num: {self.limbs_num}, joints_per_limb: {format_array_to_string(self.joints_per_limb)}') + self.joints_count = sum(self.joints_per_limb) + self.get_logger().info(f'Total limbs joints count is: {self.joints_count}') + # Hiwonder specific params + self.serial_port = self.get_parameter('hiwonder.serial_port').get_parameter_value().string_value + self.get_logger().info(f'Hiwonder specified serial port: {self.serial_port}') + self.baudrate = self.get_parameter('hiwonder.baudrate').get_parameter_value().integer_value + self.get_logger().info(f'Hiwonder serial port baudrate is: {self.baudrate}') + self.update_interval_sec = self.get_parameter('hiwonder.update_interval_sec').get_parameter_value().double_value + self.get_logger().info(f'Hiwonder update interval [sec]: {self.update_interval_sec}') + self.initial_joints_bias_degree = self.get_parameter('hiwonder.actuator_angle_bias_at_joint_zero_degree').get_parameter_value().double_array_value + self.dir = self.get_parameter('hiwonder.dir').get_parameter_value().double_array_value + self.servo_actuation_range_degree = self.get_parameter('hiwonder.servo_actuation_range_degree').get_parameter_value().double_array_value + self.servo_min_ticks = self.get_parameter('hiwonder.servo_min_ticks').get_parameter_value().integer_array_value + self.servo_max_ticks = self.get_parameter('hiwonder.servo_max_ticks').get_parameter_value().integer_array_value + self.actuation_time_ratio = self.get_parameter('hiwonder.actuation_time_ratio').get_parameter_value().double_value + self.get_logger().info(f'actuation_time_ratio specified is {self.actuation_time_ratio}') + + if len(self.initial_joints_bias_degree) != self.joints_count: + self.get_logger().error('ERROR: actuator_angle_bias_at_joint_zero_degree parameter size mismatch!') + else: + self.get_logger().info(f'Initial joints bias in degree is loaded: {format_array_to_string(self.initial_joints_bias_degree)}') + + if len(self.dir) != self.joints_count: + self.get_logger().error('ERROR: Direction array size mismatch with joints count!') + else: + self.get_logger().info(f'Direction array is loaded: {format_array_to_string(self.dir)}') + + if len(self.servo_actuation_range_degree) != self.joints_count: + self.get_logger().error('ERROR: Servo angle range array size mismatch with joints count!') + else: + self.get_logger().info(f'Servo joints angle range is loaded: {format_array_to_string(self.servo_actuation_range_degree)}') + + if len(self.servo_min_ticks) != self.joints_count: + self.get_logger().error('ERROR: Servo minimum pulse width array size mismatch!') + else: + self.get_logger().info(f'Servos minimum pulse width is loaded: {format_array_to_string(self.servo_min_ticks)}') + + if len(self.servo_max_ticks) != self.joints_count: + self.get_logger().error('ERROR: Servo maximum pulse width array size mismatch!') + else: + self.get_logger().info(f'Servos maximum pulse width is loaded: {format_array_to_string(self.servo_max_ticks)}') + + +def main(args=None): + rclpy.init(args=args) + + # Retrieve the mode from arguments + mode = 'virtual' + if len(sys.argv) > 1: + mode = sys.argv[1] + + # Create the node with the mode passed + node = PentaHiwonderActuators(mode=mode) + + # Keep the node alive to receive and process messages + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + node.get_logger().info('Beginning Hiwonder servo control, shut down with CTRL-C') + executor.spin() + except KeyboardInterrupt: + node.get_logger().info('Keyboard interrupt, shutting down.\n') + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/penta_hiwonder_actuators/resource/penta_hiwonder_actuators b/penta_hiwonder_actuators/resource/penta_hiwonder_actuators new file mode 100644 index 0000000..e69de29 diff --git a/penta_hiwonder_actuators/setup.cfg b/penta_hiwonder_actuators/setup.cfg new file mode 100644 index 0000000..550538b --- /dev/null +++ b/penta_hiwonder_actuators/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/penta_hiwonder_actuators +[install] +install_scripts=$base/lib/penta_hiwonder_actuators diff --git a/penta_hiwonder_actuators/setup.py b/penta_hiwonder_actuators/setup.py new file mode 100644 index 0000000..ef7a6a4 --- /dev/null +++ b/penta_hiwonder_actuators/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup +from glob import glob + +package_name = 'penta_hiwonder_actuators' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='modi', + maintainer_email='xx.xx@xx.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'penta_hiwonder_actuators_node = penta_hiwonder_actuators.penta_hiwonder_actuators_node:main', + ], + }, +) diff --git a/penta_hiwonder_actuators/test/test_copyright.py b/penta_hiwonder_actuators/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/penta_hiwonder_actuators/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/penta_hiwonder_actuators/test/test_flake8.py b/penta_hiwonder_actuators/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/penta_hiwonder_actuators/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/penta_hiwonder_actuators/test/test_pep257.py b/penta_hiwonder_actuators/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/penta_hiwonder_actuators/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/penta_i2c_actuators/launch/penta_i2c_actuators.launch.py b/penta_i2c_actuators/launch/penta_i2c_actuators.launch.py index 022a214..320d7b7 100644 --- a/penta_i2c_actuators/launch/penta_i2c_actuators.launch.py +++ b/penta_i2c_actuators/launch/penta_i2c_actuators.launch.py @@ -8,12 +8,25 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ # Declare the launch argument 'mode' mode_arg = DeclareLaunchArgument( 'mode', default_value='virtual', description='Choose operation mode: real or virtual' ) + ld.add_action(mode_arg) # Path to the config file config = os.path.join( @@ -28,14 +41,14 @@ def generate_launch_description(): executable='penta_i2c_actuators_node', output='screen', parameters=[config], + namespace=LaunchConfiguration('name_space'), # Pass 'mode' argument to the node - arguments=[LaunchConfiguration('mode')] + arguments=[LaunchConfiguration('mode')], + remappings=[ + ('joint_setpoints', 'joint_setpoints'), + ('actuator_setpoint_degree', 'actuator_setpoint_degree'), + ] ) - - # Create a launch description and add the actions - ld = LaunchDescription([ - mode_arg, # Add the mode argument - i2c_actuators_node # Add the node - ]) + ld.add_action(i2c_actuators_node) return ld diff --git a/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py b/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py index 14714eb..35cf1d0 100644 --- a/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py +++ b/penta_i2c_actuators/penta_i2c_actuators/penta_i2c_actuators_node.py @@ -4,6 +4,10 @@ import math import sys +class Mode: + REAL = 'real' + VIRTUAL = 'virtual' + class PentaI2CActuators(Node): def __init__(self, mode): super().__init__('penta_i2c_actuators') @@ -16,6 +20,11 @@ def __init__(self, mode): # Set mode (real or virtual) self.real_mode_flag = True + if mode == Mode.VIRTUAL: + self.real_mode_flag = False + self.get_logger().info('Running in virtual mode, no actuators will be controlled') + else: + self.get_logger().info('Running in real mode, actuators will be controlled') if self.real_mode_flag: from adafruit_servokit import ServoKit self.kit = ServoKit(channels=16) # Use 16-channel board @@ -24,10 +33,10 @@ def __init__(self, mode): self.kit.servo[i].actuation_range = self.servo_actuation_range_degree[i] # Publisher actuators setpoint from joint states - self.setpoint_publisher_ = self.create_publisher(JointState, '/actuator_setpoint_degree', 10) + self.setpoint_publisher_ = self.create_publisher(JointState, 'actuator_setpoint_degree', 10) self.joint_states_subscriber = self.create_subscription( JointState, - "/joint_states", + "joint_setpoints", lambda msg : self.on_joint_states_callback(msg), 10 # Set QoS to 10 ) @@ -157,7 +166,7 @@ def main(args=None): rclpy.init(args=args) # Retrieve the mode from arguments - mode = 'virtual' + mode = Mode.VIRTUAL if len(sys.argv) > 1: mode = sys.argv[1] diff --git a/penta_pod/launch/penta_pod.launch.py b/penta_pod/launch/penta_pod.launch.py index 35fedf0..3796749 100644 --- a/penta_pod/launch/penta_pod.launch.py +++ b/penta_pod/launch/penta_pod.launch.py @@ -7,6 +7,9 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + def get_robot_limbs_num_from_yaml(package_name, config_folder_name, config_file_name): message = """ @@ -27,6 +30,18 @@ def get_robot_limbs_num_from_yaml(package_name, config_folder_name, config_file_ def generate_launch_description(): + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ package_name = "penta_description" config_folder_name = "config" config_file_name = "limb_0_mdh.yaml" @@ -36,25 +51,48 @@ def generate_launch_description(): config_file_name, ) config_file_name = "general_config.yaml" - limbs_num = get_robot_limbs_num_from_yaml(package_name, config_folder_name, config_file_name) - ld = LaunchDescription() + + limbs_num = get_robot_limbs_num_from_yaml( + package_name, config_folder_name, config_file_name + ) + for i in range(limbs_num): limb_prefix = "limb" + str(i) temp_node = Node( package="limb_kin_chain", executable="limb_kin_chain_node", - namespace=limb_prefix, - name=limb_prefix, + namespace=LaunchConfiguration("name_space"), + name=limb_prefix + "_kin_chain_node", output="screen", parameters=[config_limb0], - # remappings=[(individual_joint_state_topic, '/joint_states')] + remappings=[ + ( + "joint_states", + limb_prefix + "/joint_states", + ), + ( + "joint_setpoints", + limb_prefix + "/joint_setpoints", + ), + ( + "xyz_msg", + limb_prefix + "/xyz_msg", + ) + ], ) ld.add_action(temp_node) # Added the Node to LaunchDescription # Include the joystick teleoperation launch file joystick_teleop_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(FindPackageShare('penta_teleop').find('penta_teleop'), 'launch', 'teleop_joystick.launch.py') - ) + os.path.join( + FindPackageShare("penta_teleop").find("penta_teleop"), + "launch", + "teleop_joystick.launch.py", + ) + ), + launch_arguments={ + "name_space": LaunchConfiguration("name_space"), + }.items(), ) ld.add_action(joystick_teleop_launch) return ld diff --git a/penta_pod/launch/penta_rviz.launch.py b/penta_pod/launch/penta_rviz.launch.py index 6b8185e..ef86105 100644 --- a/penta_pod/launch/penta_rviz.launch.py +++ b/penta_pod/launch/penta_rviz.launch.py @@ -8,26 +8,49 @@ from launch.substitutions import Command, LaunchConfiguration from launch import LaunchDescription import os +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + # This is a launch rviz2 on a laptop for the toperware_bot def generate_launch_description(): + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + + robot_state_publisher_remapping_arg = DeclareLaunchArgument( + 'joint_states_remappings', + default_value='/joint_states', + description='Robot state publisher input topic' + ) + rviz_config_file = PathJoinSubstitution( [FindPackageShare('penta_description'), 'config', 'penta.rviz']) - urdf_path = os.path.join( - get_package_share_directory("penta_description"), - "urdf", - "penta.urdf.xacro", - ) - model_arg = DeclareLaunchArgument(name='model', default_value=str(urdf_path), - description='Absolute path to robot urdf file') - robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]), - value_type=str) - return LaunchDescription([ - DeclareLaunchArgument( + rviz_config_file_arg = DeclareLaunchArgument( 'config_file', default_value=rviz_config_file, description='Path to the RViz configuration file' + ) + + penta_description_pkg = get_package_share_directory("penta_description") + + # RViz + robot_state_publisher_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(penta_description_pkg, "launch", "robot_state_publisher.launch.py") ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + 'joint_states_remappings': LaunchConfiguration('joint_states_remappings'), + }.items() + ) + return LaunchDescription([ + name_space_arg, + robot_state_publisher_remapping_arg, + rviz_config_file_arg, Node( package='rviz2', executable='rviz2', @@ -35,14 +58,5 @@ def generate_launch_description(): output='screen', arguments=['-d', LaunchConfiguration('config_file')] ), - model_arg, - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[ - {'robot_description': robot_description} - ], - ), + robot_state_publisher_launch, ]) \ No newline at end of file diff --git a/penta_pod/launch/penta_sim_full_rviz.launch.py b/penta_pod/launch/penta_sim_full_rviz.launch.py index b5a6667..6d474a1 100644 --- a/penta_pod/launch/penta_sim_full_rviz.launch.py +++ b/penta_pod/launch/penta_sim_full_rviz.launch.py @@ -4,59 +4,92 @@ from launch_ros.substitutions import FindPackageShare from launch.substitutions import LaunchConfiguration import os +from launch.actions import DeclareLaunchArgument +from launch import LaunchDescription def generate_launch_description(): # Include scorption_cmd_vel_mux launcer - scorption_cmd_vel_mux_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(FindPackageShare('scorpion_cmd_vel_mux').find('scorpion_cmd_vel_mux'), 'launch', 'cmd_vel_mux.launch.py') - ) + # scorption_cmd_vel_mux_launch = IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # os.path.join(FindPackageShare('scorpion_cmd_vel_mux').find('scorpion_cmd_vel_mux'), 'launch', 'cmd_vel_mux.launch.py') + # ) + # ) + + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", ) + ld.add_action(name_space_arg) + """ Nodes """ # Include RVIZ launch file rviz_penta_pod_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_rviz.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + 'joint_states_remappings': 'joint_setpoints', + }.items() ) - # Include the penta_pod launch file + # Include the penta_pod launch file (limbs) penta_pod_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_pod.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) # Include the gait_generator launch file gait_generator_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('gait_generator').find('gait_generator'), 'launch', 'gait_generator.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) # Include the joints_aggregator launch file joints_aggregator_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('joints_aggregator').find('joints_aggregator'), 'launch', 'joints_aggregator.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) # Include the null space cmd publisher launch file null_space_publisher = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) # Include the twerk action server launch file twerk_action_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) return LaunchDescription([ - scorption_cmd_vel_mux_launch, + # scorption_cmd_vel_mux_launch, + name_space_arg, rviz_penta_pod_launch, penta_pod_launch, gait_generator_launch, diff --git a/penta_pod/launch/penta_sim_rviz.launch.py b/penta_pod/launch/penta_sim_rviz.launch.py index 8335d96..ee67eff 100644 --- a/penta_pod/launch/penta_sim_rviz.launch.py +++ b/penta_pod/launch/penta_sim_rviz.launch.py @@ -10,7 +10,10 @@ def generate_launch_description(): rviz_penta_pod_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_rviz.launch.py') - ) + ), + launch_arguments={ + 'joint_states_remappings': '/joint_setpoints', + }.items() ) # Include the penta_pod launch file @@ -34,25 +37,9 @@ def generate_launch_description(): ) ) - # Include the null space cmd publisher launch file - # null_space_publisher = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py') - # ) - # ) - - # Include the twerk action server launch file - # twerk_action_server = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py') - # ) - # ) - return LaunchDescription([ rviz_penta_pod_launch, penta_pod_launch, gait_generator_launch, joints_aggregator_launch, - # null_space_publisher, - # twerk_action_server ]) diff --git a/penta_pod/launch/realhardware_bringup.launch.py b/penta_pod/launch/realhardware_bringup.launch.py index be9c69d..b607e98 100644 --- a/penta_pod/launch/realhardware_bringup.launch.py +++ b/penta_pod/launch/realhardware_bringup.launch.py @@ -2,23 +2,42 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, EqualsSubstitution +from launch.conditions import IfCondition import os def generate_launch_description(): + + """ Launch args """ + # Declare the 'name_space' argument with a default value of '' + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + # Declare the 'mode' argument with a default value of 'real' mode_arg = DeclareLaunchArgument( 'mode', default_value='real', description='Mode to run the actuators (real or sim)' ) - mode = LaunchConfiguration('mode') + + motors_interface_arg = DeclareLaunchArgument( + 'motors_interface', + default_value='i2c', + description='Type of motors to use (i2c or hiwonder)' + ) + motors_interface = LaunchConfiguration('motors_interface') # Include the penta_pod launch file penta_pod_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'penta_pod.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) # Include the gait_generator launch file @@ -32,37 +51,67 @@ def generate_launch_description(): joints_aggregator_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('joints_aggregator').find('joints_aggregator'), 'launch', 'joints_aggregator.launch.py') - ) + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() ) - # Include the penta_i2c_actuators launch file with the mode argument + # Include the penta_i2c_actuators launch file with the mode argument (when motors_interface != hiwonder) penta_i2c_actuators_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(FindPackageShare('penta_i2c_actuators').find('penta_i2c_actuators'), 'launch', 'penta_i2c_actuators.launch.py') ), - launch_arguments={'mode': mode}.items() # Pass the mode argument + launch_arguments={ + 'mode': LaunchConfiguration('mode'), + 'name_space': LaunchConfiguration('name_space'), + }.items(), # Pass the argyments + condition=IfCondition(EqualsSubstitution(motors_interface, 'i2c')) + ) + + # Include the penta_hiwonder_actuators launch file with the mode argument (when motors_interface == hiwonder) + penta_hiwonder_actuators_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(FindPackageShare('penta_hiwonder_actuators').find('penta_hiwonder_actuators'), 'launch', 'penta_hiwonder_actuators.launch.py') + ), + launch_arguments={ + 'mode': LaunchConfiguration('mode'), + 'name_space': LaunchConfiguration('name_space'), + 'joints_setpoint_topic': 'joint_setpoints', + }.items(), # Pass the mode argument + condition=IfCondition(EqualsSubstitution(motors_interface, 'hiwonder')) ) # Include the null space cmd publisher launch file - # null_space_publisher = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py') - # ) - # ) + # Include the null space cmd publisher launch file + null_space_publisher = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'null_pose_publisher.launch.py') + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() + ) # Include the twerk action server launch file - # twerk_action_server = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py') - # ) - # ) + twerk_action_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(FindPackageShare('base_twerk').find('base_twerk'), 'launch', 'base_twerk_action_server.launch.py') + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() + ) return LaunchDescription([ + name_space_arg, mode_arg, + motors_interface_arg, penta_pod_launch, gait_generator_launch, joints_aggregator_launch, penta_i2c_actuators_launch, - # null_space_publisher, - # twerk_action_server + penta_hiwonder_actuators_launch, + null_space_publisher, + twerk_action_server ]) diff --git a/penta_pod/package.xml b/penta_pod/package.xml index e0b6f7f..635de0f 100644 --- a/penta_pod/package.xml +++ b/penta_pod/package.xml @@ -12,9 +12,11 @@ gait_generator joints_aggregator base_twerk + penta_teleop penta_i2c_actuators - + penta_hiwonder_actuators + robot_state_publisher ament_lint_auto diff --git a/penta_teleop/CMakeLists.txt b/penta_teleop/CMakeLists.txt index 5c06980..44967ea 100644 --- a/penta_teleop/CMakeLists.txt +++ b/penta_teleop/CMakeLists.txt @@ -11,11 +11,14 @@ find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(base_twerk_msgs REQUIRED) find_package(gait_generator_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(commons REQUIRED) # Declare library add_library(${PROJECT_NAME} src/${PROJECT_NAME}/joystick_extra_controls.cpp src/${PROJECT_NAME}/declare_load_parameters.cpp + src/${PROJECT_NAME}/cmd_vel_smoothing.cpp ) target_include_directories(${PROJECT_NAME} @@ -28,6 +31,8 @@ ament_target_dependencies(${PROJECT_NAME} sensor_msgs base_twerk_msgs gait_generator_msgs + rclcpp_action + commons ) add_executable(joystick_extra_controls_node src/joystick_extra_controls_node.cpp) @@ -35,6 +40,11 @@ target_link_libraries(joystick_extra_controls_node ${PROJECT_NAME} ) +add_executable(cmd_vel_smoothing_node src/cmd_vel_smoothing_node.cpp) +target_link_libraries(cmd_vel_smoothing_node + ${PROJECT_NAME} + ) + # Install install( DIRECTORY include/ @@ -43,6 +53,7 @@ install( install(TARGETS joystick_extra_controls_node + cmd_vel_smoothing_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/penta_teleop/README.md b/penta_teleop/README.md index 861429b..5f39f4f 100644 --- a/penta_teleop/README.md +++ b/penta_teleop/README.md @@ -8,18 +8,25 @@ Tested with wirless Xbox contorller provided with a dongle Connect the dongle to Raspberry PI, to control the robot as follows: -1- Left joystick for linear motion in the plane (x and y) +1- `Left joystick` for linear motion in the plane (x and y) -2- Right joystick for rotations +2- `Right joystick` for rotations -3- D-Pad up and down buttons, to move the robot up and down +3- `D-Pad` up and down buttons, to move the robot up and down + +4- `X button` to change walking pattern + +5- `A button` to change twerk mode + +6- `B button` to start executing the twerk motion + +7- `D-pad + Right Trigger button (RT)` allow the robot to pitch -4- X button to change walking pattern Note, make sure to enable the motion using: -1- Use Right Trigger button (RT) to enable the turbo mode +1- `Right Trigger button (RT)` to enable walking in turbo mode -2- Use Left Trigger button (LT) to enable slow motion mode +2- `Left Trigger button (LT)` to enable waling in slow mode 3- Walking pattern changes only when the robot is in stand-still \ No newline at end of file diff --git a/penta_teleop/config/cmd_vel_smoothing.yaml b/penta_teleop/config/cmd_vel_smoothing.yaml new file mode 100644 index 0000000..3d43889 --- /dev/null +++ b/penta_teleop/config/cmd_vel_smoothing.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + max_linear_acceleration: 0.1 # m/s^2 + max_angular_acceleration: 1.0 # rad/s^2 + publishing_rate: 100.0 # hz + input_cmd_vel_timeout_sec: 0.2 # seconds \ No newline at end of file diff --git a/penta_teleop/config/joy_extra_controls.yaml b/penta_teleop/config/joy_extra_controls.yaml index 5862a81..2fbdc00 100644 --- a/penta_teleop/config/joy_extra_controls.yaml +++ b/penta_teleop/config/joy_extra_controls.yaml @@ -6,3 +6,20 @@ z_base_max_value: 0.15 gait_patterns_ctl: button_index: 2 + twerk_it: # twerk action client goal parameters + switch_mode_button_index: 0 + trigger_button_index: 1 + w: 2.0 + dance_time_millis: 10000 + twerk_mode_names: + - "linear_up_and_down" + - "angular_roll" + - "linear_left_and_right" + twerk_mode_axes: + - 2 # for z + - 3 # for roll + - 1 # for y + twerk_mode_magnitudes: + - 0.02 # meter z + - 0.1 # rad roll + - 0.02 # meter y diff --git a/penta_teleop/include/penta_teleop/cmd_vel_smoothing.hpp b/penta_teleop/include/penta_teleop/cmd_vel_smoothing.hpp new file mode 100644 index 0000000..3f6f858 --- /dev/null +++ b/penta_teleop/include/penta_teleop/cmd_vel_smoothing.hpp @@ -0,0 +1,47 @@ +#ifndef CMD_VEL_SMOOTHING_HPP +#define CMD_VEL_SMOOTHING_HPP + +#include +#include + + +namespace penta_pod::teleop::twist_smoothing +{ + +class CmdVelSmoothing +{ + using Twist = geometry_msgs::msg::Twist; + +public: + CmdVelSmoothing(rclcpp::Node::SharedPtr node); + + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Publisher::SharedPtr smoothed_cmd_vel_pub_; + Twist::SharedPtr target_cmd_vel_; + Twist::SharedPtr smoothed_cmd_vel_; + + struct node_parameters { + double max_linear_acceleration; + double max_angular_acceleration; + double publishing_rate; + double input_cmd_vel_timeout_sec; + } params_; + double last_cmd_vel_time_; + + double smoothenScalar(double target, double current, double acc, double timer_interval_sec); + auto smoothenTwist(const Twist& target_cmd_vel, const Twist& current_cmd_vel, double timer_interval_sec) -> Twist::SharedPtr; + + + void declare_parameters(); + void load_parameters(); + void cmdVelCallback(const Twist::SharedPtr msg); +}; + +} // namespace penta_pod::teleop::twist_smoothing + +#endif // CMD_VEL_SMOOTHING_HPP \ No newline at end of file diff --git a/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp b/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp index 34eaec5..f590a09 100644 --- a/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp +++ b/penta_teleop/include/penta_teleop/joystick_extra_controls.hpp @@ -3,6 +3,7 @@ #include "rclcpp/rclcpp.hpp" #include +#include // include messages #include "base_twerk_msgs/srv/base_pose_setpoint.hpp" @@ -10,6 +11,7 @@ #include "gait_generator_msgs/srv/set_gait_pattern.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/joy.hpp" +#include "base_twerk_msgs/action/base_twerk_action.hpp" namespace penta_pod::teleop::joystick_extra_controls { @@ -17,6 +19,8 @@ using PoseStamped = geometry_msgs::msg::PoseStamped; using BasePoseSetpoint = base_twerk_msgs::srv::BasePoseSetpoint; using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose; using SetGaitPattern = gait_generator_msgs::srv::SetGaitPattern; +using BaseTwerkAction = base_twerk_msgs::action::BaseTwerkAction; +using GoalHandleBaseTwerkAction = rclcpp_action::ClientGoalHandle; class JoystickExtraControls { private: @@ -27,28 +31,120 @@ class JoystickExtraControls { rclcpp::Client::SharedPtr set_base_pose_client_; rclcpp::Client::SharedPtr set_gait_pattern_client_; + rclcpp_action::Client::SharedPtr base_twerk_action_client_; + rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::TimerBase::SharedPtr timer_; + /* up down motion, funs/vars */ + int d_pad_up_down_axis_index_{7}; + int gait_patterns_ctl_button_index_{2}; + int num_of_gait_patterns_{3}; + double z_base_max_value_{0.01}; + double z_base_min_value_{0.15}; + void joy_sub_callback(const sensor_msgs::msg::Joy::SharedPtr msg); void dpad_up_down(const sensor_msgs::msg::Joy::SharedPtr msg); void set_gait_pattern(const sensor_msgs::msg::Joy::SharedPtr msg); + /* get/set base pose */ void handle_get_base_pose_response( rclcpp::Client::SharedFuture response, - double z_disp_base_command); + double z_disp_base_command, double pitch_disp_base_command); void handle_set_base_pose_response( rclcpp::Client::SharedFuture response); + /* twerk funs */ + void send_base_twerk_goal(int twerk_axis_index, double twerk_magnitude, int twerk_time_millis); + + bool is_null_space_motion_possible_{true}; + void check_twerk_it_mode_switch(const sensor_msgs::msg::Joy::SharedPtr msg); + void check_twerk_pressed(const sensor_msgs::msg::Joy::SharedPtr msg); + + + /* parameters funs */ void declare_parameters(); bool get_parameters(); + void declare_base_height_parameters(); + bool get_base_height_parameters(); + void declare_gait_patterns_parameters(); + bool get_gait_patterns_parameters(); + void declare_twerk_it_parameters(); + bool get_twerk_it_parameters(); - int d_pad_up_down_axis_index_{7}; - int gait_patterns_ctl_button_index_{2}; - int num_of_gait_patterns_{3}; - double z_base_max_value_{0.01}; - double z_base_min_value_{0.15}; + /* twerk it params struct */ + struct TwerkItParams { + int trigger_button_index{0}; // button to trigger twerk + int switch_mode_button_index{1}; // button to switch twerk modes + double w{4.0}; // rad/s + int dance_time_millis{10000}; // milliseconds + int current_twerk_mode_index{0}; + + std::vector magnitudes; + std::vector twerk_mode_names; + std::vector twerk_mode_axes; + + /* get joystick buttons indicies*/ + int get_twerk_it_trigger_button_index() { + return trigger_button_index; + } + + int get_twerk_it_switch_mode_button_index() { + return switch_mode_button_index; + } + + /* get parameters */ + auto check_current_twerk_it_mode_index() -> bool { + if (current_twerk_mode_index < 0 || + static_cast(current_twerk_mode_index) >= twerk_mode_names.size()) { + RCLCPP_ERROR(rclcpp::get_logger("JoystickExtraControls"), + "Current twerk mode index is out of bounds : %d resetting it to zero!", + current_twerk_mode_index); + current_twerk_mode_index = 0; + return false; + } + return true; + } + + auto get_current_twerk_it_mode_name() -> std::string { + if(!check_current_twerk_it_mode_index()) { + RCLCPP_ERROR(rclcpp::get_logger("fun get_current_twerk_it_mode_name"), + "Failed to assert current twerk mode index. Out of bounds"); + } + return twerk_mode_names[current_twerk_mode_index]; + } + + auto get_current_twerk_it_axis() -> int { + if(!check_current_twerk_it_mode_index()) { + RCLCPP_ERROR(rclcpp::get_logger("fun get_current_twerk_it_axis"), + "Failed to assert current twerk mode index. Out of bounds"); + } + return twerk_mode_axes[current_twerk_mode_index]; + } + + auto get_current_twerk_it_magnitude() -> double { + if(!check_current_twerk_it_mode_index()) { + RCLCPP_ERROR(rclcpp::get_logger("fun get_current_twerk_it_magnitude"), + "Failed to assert current twerk mode index. Out of bounds"); + } + return magnitudes[current_twerk_mode_index]; + } + + int get_current_twerk_it_dance_time_millis() { + return dance_time_millis; + } + + /* set some stuff */ + int switch_twerk_it_mode() { + current_twerk_mode_index++; + if (static_cast(current_twerk_mode_index) >= twerk_mode_names.size()) { + current_twerk_mode_index = 0; + } + return current_twerk_mode_index; + } + + } twerk_it_params_; public: explicit JoystickExtraControls(); diff --git a/penta_teleop/launch/teleop_joystick.launch.py b/penta_teleop/launch/teleop_joystick.launch.py index 24d6f2a..026bdb1 100644 --- a/penta_teleop/launch/teleop_joystick.launch.py +++ b/penta_teleop/launch/teleop_joystick.launch.py @@ -3,10 +3,23 @@ import os from ament_index_python.packages import get_package_share_directory - +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + ld = LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + """ Nodes """ + joy_params = os.path.join( get_package_share_directory("penta_teleop"), "config", "teleop_joystick.yaml" ) @@ -14,6 +27,7 @@ def generate_launch_description(): joy_node = Node( package="joy", executable="joy_node", + namespace=LaunchConfiguration("name_space"), parameters=[joy_params], ) @@ -21,8 +35,9 @@ def generate_launch_description(): package="teleop_twist_joy", executable="teleop_node", name="teleop_node", + namespace=LaunchConfiguration("name_space"), parameters=[joy_params], - # remappings=[("/cmd_vel", "/input/cmd_vel_teleop_joy")], + remappings=[("cmd_vel", "cmd_vel_not_smoothed")], ) joystick_extra_controls_configs = os.path.join( @@ -37,9 +52,37 @@ def generate_launch_description(): package="penta_teleop", executable="joystick_extra_controls_node", name="joystick_extra_controls_node", + namespace=LaunchConfiguration("name_space"), + remappings=[ + # services + ("cmd_null_setpoint", "base_twerk/cmd_null_setpoint"), + ("get_current_null_pose", "base_twerk/get_current_null_pose"), + ], parameters=[joystick_extra_controls_configs, general_config_params], ) - return LaunchDescription( - [joy_node, teleop_twist_joy_node, joystick_extra_controls_node] + cmd_vel_smoothing_configs = os.path.join( + get_package_share_directory("penta_teleop"), "config", "cmd_vel_smoothing.yaml" ) + cmd_vel_smoothing_node = Node( + package="penta_teleop", + executable="cmd_vel_smoothing_node", + name="cmd_vel_smoothing_node", + namespace=LaunchConfiguration("name_space"), + remappings=[ + ("cmd_vel", "cmd_vel_not_smoothed"), + ("smoothed_cmd_vel", "cmd_vel"), + ], + parameters=[cmd_vel_smoothing_configs], + ) + + nodes = [ + joy_node, + teleop_twist_joy_node, + joystick_extra_controls_node, + cmd_vel_smoothing_node, + ] + for node in nodes: + ld.add_action(node) + + return ld diff --git a/penta_teleop/package.xml b/penta_teleop/package.xml index b4040a8..413ed40 100644 --- a/penta_teleop/package.xml +++ b/penta_teleop/package.xml @@ -14,6 +14,7 @@ teleop_twist_joy base_twerk_msgs gait_generator_msgs + rclcpp_action ament_cmake diff --git a/penta_teleop/src/cmd_vel_smoothing_node.cpp b/penta_teleop/src/cmd_vel_smoothing_node.cpp new file mode 100644 index 0000000..29163bf --- /dev/null +++ b/penta_teleop/src/cmd_vel_smoothing_node.cpp @@ -0,0 +1,19 @@ +#include "penta_teleop/cmd_vel_smoothing.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + // Create a shared node instance + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("twist_smoothing_node"); + + using penta_pod::teleop::twist_smoothing::CmdVelSmoothing; + auto twist_smoothing = CmdVelSmoothing(node); + + // Spin + rclcpp::spin(node); + + // Cleanup + rclcpp::shutdown(); + return 0; +} diff --git a/penta_teleop/src/penta_teleop/cmd_vel_smoothing.cpp b/penta_teleop/src/penta_teleop/cmd_vel_smoothing.cpp new file mode 100644 index 0000000..4f1ada9 --- /dev/null +++ b/penta_teleop/src/penta_teleop/cmd_vel_smoothing.cpp @@ -0,0 +1,111 @@ +#include "penta_teleop/cmd_vel_smoothing.hpp" + +namespace penta_pod::teleop::twist_smoothing +{ + +CmdVelSmoothing::CmdVelSmoothing(rclcpp::Node::SharedPtr node) + : node_(node) { + // Parameters for maximum acceleration + declare_parameters(); + load_parameters(); + smoothed_cmd_vel_ = std::make_shared(); + last_cmd_vel_time_ = node_->now().seconds(); + + // Subscriber for raw cmd_vel + cmd_vel_sub_ = node_->create_subscription( + "cmd_vel", 10, + [this](const Twist::SharedPtr msg) { cmdVelCallback(msg); }); + + // Publisher for smoothed cmd_vel + smoothed_cmd_vel_pub_ = node_->create_publisher( + "smoothed_cmd_vel", 10); + + // Timer for periodic smoothing (if needed) + auto timer_interval = static_cast(1000.0 / params_.publishing_rate); + timer_ = node_->create_wall_timer( + std::chrono::milliseconds(timer_interval), + [this]() { + // Check for timeout if new target is not received + if (node_->now().seconds() - last_cmd_vel_time_ > params_.input_cmd_vel_timeout_sec) { + target_cmd_vel_ = std::make_shared(); // zero twist on timeout + } + // Filter and publish the smoothed cmd_vel + if (target_cmd_vel_) { + double timer_interval = static_cast(1000.0 / params_.publishing_rate); + smoothed_cmd_vel_ = smoothenTwist(*target_cmd_vel_, *smoothed_cmd_vel_, timer_interval / 1000.0); + } + smoothed_cmd_vel_pub_->publish(*smoothed_cmd_vel_); + }); +} + + +void CmdVelSmoothing::cmdVelCallback(const Twist::SharedPtr msg) +{ + last_cmd_vel_time_ = node_->now().seconds(); + target_cmd_vel_ = msg; +} + +double CmdVelSmoothing::smoothenScalar(double target, double current, double acc, double timer_interval_sec) { + double max_step = acc * timer_interval_sec; + double diff = target - current; + if (std::abs(diff) > max_step) { + diff = (diff > 0 ? max_step : -max_step); + } + return current + diff; +} + +auto CmdVelSmoothing::smoothenTwist(const Twist& target_cmd_vel, const Twist& current_cmd_vel, double timer_interval_sec) -> Twist::SharedPtr { + + double dt = timer_interval_sec; + + Twist::SharedPtr smoothed_cmd_vel = std::make_shared(); + + // Smooth linear.x + smoothed_cmd_vel->linear.x = smoothenScalar(target_cmd_vel.linear.x, current_cmd_vel.linear.x, params_.max_linear_acceleration, dt); + // Smooth linear.y + smoothed_cmd_vel->linear.y = smoothenScalar(target_cmd_vel.linear.y, current_cmd_vel.linear.y, params_.max_linear_acceleration, dt); + + // Smooth angular.z + smoothed_cmd_vel->angular.z = smoothenScalar(target_cmd_vel.angular.z, current_cmd_vel.angular.z, params_.max_angular_acceleration, dt); + + return smoothed_cmd_vel; +} + +void CmdVelSmoothing::declare_parameters() { + node_->declare_parameter("publishing_rate"); + node_->declare_parameter("max_linear_acceleration"); + node_->declare_parameter("max_angular_acceleration"); + node_->declare_parameter("input_cmd_vel_timeout_sec"); +} + +void CmdVelSmoothing::load_parameters() { + double publishing_rate_default = 10.0; + if (!node_->get_parameter("publishing_rate", publishing_rate_default)) { + RCLCPP_WARN(node_->get_logger(), "Failed to load 'publishing_rate', using default: %f", publishing_rate_default); + } else { + params_.publishing_rate = publishing_rate_default; + } + + double max_linear_acceleration_default = 0.5; + if (!node_->get_parameter("max_linear_acceleration", max_linear_acceleration_default)) { + RCLCPP_WARN(node_->get_logger(), "Failed to load 'max_linear_acceleration', using default: %f", max_linear_acceleration_default); + } else { + params_.max_linear_acceleration = max_linear_acceleration_default; + } + + double max_angular_acceleration_default = 1.0; + if (!node_->get_parameter("max_angular_acceleration", max_angular_acceleration_default)) { + RCLCPP_WARN(node_->get_logger(), "Failed to load 'max_angular_acceleration', using default: %f", max_angular_acceleration_default); + } else { + params_.max_angular_acceleration = max_angular_acceleration_default; + } + + double input_cmd_vel_timeout_sec_default = 0.2; + if (!node_->get_parameter("input_cmd_vel_timeout_sec", input_cmd_vel_timeout_sec_default)) { + RCLCPP_WARN(node_->get_logger(), "Failed to load 'input_cmd_vel_timeout_sec', using default: %f", input_cmd_vel_timeout_sec_default); + } else { + params_.input_cmd_vel_timeout_sec = input_cmd_vel_timeout_sec_default; + } +} + +} // namespace penta_pod::teleop::twist_smoothing \ No newline at end of file diff --git a/penta_teleop/src/penta_teleop/declare_load_parameters.cpp b/penta_teleop/src/penta_teleop/declare_load_parameters.cpp index e83e78f..48efc27 100644 --- a/penta_teleop/src/penta_teleop/declare_load_parameters.cpp +++ b/penta_teleop/src/penta_teleop/declare_load_parameters.cpp @@ -3,61 +3,172 @@ namespace penta_pod::teleop::joystick_extra_controls { void JoystickExtraControls::declare_parameters() { + this->declare_base_height_parameters(); + this->declare_gait_patterns_parameters(); + this->declare_twerk_it_parameters(); +} + +bool JoystickExtraControls::get_parameters() { + bool height_params_load_success = this->get_base_height_parameters(); + + return height_params_load_success && + this->get_gait_patterns_parameters() && + this->get_twerk_it_parameters(); +} + +void JoystickExtraControls::declare_base_height_parameters() { node_->declare_parameter("base_height_ctl.d_pad_up_down_axis_index"); - node_->declare_parameter("gait_patterns_ctl.button_index"); - node_->declare_parameter("gait_parameters.gait_patterns.num"); - node_->declare_parameter("base_height_ctl.z_base_min_value"); node_->declare_parameter("base_height_ctl.z_base_max_value"); + node_->declare_parameter("base_height_ctl.z_base_min_value"); + } -bool JoystickExtraControls::get_parameters() { +bool JoystickExtraControls::get_base_height_parameters() { std::string param_name; param_name = "base_height_ctl.d_pad_up_down_axis_index"; if (!node_->get_parameter(param_name, this->d_pad_up_down_axis_index_)) { - RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s", + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", param_name.c_str()); return false; } RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d", param_name.c_str(), this->d_pad_up_down_axis_index_); + param_name = "base_height_ctl.z_base_max_value"; + if (!node_->get_parameter(param_name, this->z_base_max_value_)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f", + param_name.c_str(), this->z_base_max_value_); + + param_name = "base_height_ctl.z_base_min_value"; + if (!node_->get_parameter(param_name, this->z_base_min_value_)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f", + param_name.c_str(), this->z_base_min_value_); + + return true; // Ensure the function always returns a value +} + +void JoystickExtraControls::declare_gait_patterns_parameters() { + node_->declare_parameter("gait_patterns_ctl.button_index"); +} + +bool JoystickExtraControls::get_gait_patterns_parameters() { + std::string param_name; + param_name = "gait_patterns_ctl.button_index"; - if (!node_->get_parameter(param_name, - this->gait_patterns_ctl_button_index_)) { - RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s", + if (!node_->get_parameter(param_name, this->gait_patterns_ctl_button_index_)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", param_name.c_str()); return false; } RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d", param_name.c_str(), this->gait_patterns_ctl_button_index_); - param_name = "gait_parameters.gait_patterns.num"; - if (!node_->get_parameter(param_name, this->num_of_gait_patterns_)) { - RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s", + return true; +} + +void JoystickExtraControls::declare_twerk_it_parameters() { + node_->declare_parameter("twerk_it.switch_mode_button_index"); + node_->declare_parameter("twerk_it.trigger_button_index"); + node_->declare_parameter("twerk_it.w"); + node_->declare_parameter("twerk_it.dance_time_millis"); + + node_->declare_parameter>("twerk_it.twerk_mode_names", + std::vector{}); + node_->declare_parameter>("twerk_it.twerk_mode_axes", + std::vector{}); + node_->declare_parameter>("twerk_it.twerk_mode_magnitudes", + std::vector{}); +} + +bool JoystickExtraControls::get_twerk_it_parameters() { + std::string param_name; + + param_name = "twerk_it.switch_mode_button_index"; + if (!node_->get_parameter(param_name, twerk_it_params_.switch_mode_button_index)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d", + param_name.c_str(), twerk_it_params_.switch_mode_button_index); + + param_name = "twerk_it.trigger_button_index"; + if (!node_->get_parameter(param_name, twerk_it_params_.trigger_button_index)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", param_name.c_str()); return false; } RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d", - param_name.c_str(), this->num_of_gait_patterns_); + param_name.c_str(), twerk_it_params_.trigger_button_index); - param_name = "base_height_ctl.z_base_max_value"; - if (!node_->get_parameter(param_name, this->z_base_max_value_)) { - RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s", + param_name = "twerk_it.w"; + if (!node_->get_parameter(param_name, twerk_it_params_.w)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", param_name.c_str()); return false; } RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f", - param_name.c_str(), this->z_base_max_value_); + param_name.c_str(), twerk_it_params_.w); - param_name = "base_height_ctl.z_base_min_value"; - if (!node_->get_parameter(param_name, this->z_base_min_value_)) { - RCLCPP_ERROR(node_->get_logger(), "Can not load parameters %s", + param_name = "twerk_it.dance_time_millis"; + if (!node_->get_parameter(param_name, twerk_it_params_.dance_time_millis)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", param_name.c_str()); return false; } - RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %f", - param_name.c_str(), this->z_base_min_value_); + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with value %d", + param_name.c_str(), twerk_it_params_.dance_time_millis); + + param_name = "twerk_it.twerk_mode_names"; + if (!node_->get_parameter(param_name, twerk_it_params_.twerk_mode_names)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with %lu values", + param_name.c_str(), twerk_it_params_.twerk_mode_names.size()); + + param_name = "twerk_it.twerk_mode_axes"; + if (!node_->get_parameter(param_name, twerk_it_params_.twerk_mode_axes)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with %lu values", + param_name.c_str(), twerk_it_params_.twerk_mode_axes.size()); + + param_name = "twerk_it.twerk_mode_magnitudes"; + if (!node_->get_parameter(param_name, twerk_it_params_.magnitudes)) { + RCLCPP_ERROR(node_->get_logger(), "Cannot load parameter %s", + param_name.c_str()); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Loaded parameter %s with %lu values", + param_name.c_str(), twerk_it_params_.magnitudes.size()); + + if (twerk_it_params_.twerk_mode_names.size() != + twerk_it_params_.twerk_mode_axes.size() || + twerk_it_params_.twerk_mode_names.size() != + twerk_it_params_.magnitudes.size()) { + RCLCPP_ERROR(node_->get_logger(), + "twerk it parameters sizes do not match. " + "twerk_mode_names: %lu, " + "twerk_mode_axes: %lu, " + "magnitudes: %lu", + twerk_it_params_.twerk_mode_names.size(), + twerk_it_params_.twerk_mode_axes.size(), + twerk_it_params_.magnitudes.size()); + return false; + } return true; } diff --git a/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp b/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp index 4dfdd5b..2349ca2 100644 --- a/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp +++ b/penta_teleop/src/penta_teleop/joystick_extra_controls.cpp @@ -1,4 +1,5 @@ #include "penta_teleop/joystick_extra_controls.hpp" +#include "commons/quaternion_utils.hpp" namespace penta_pod::teleop::joystick_extra_controls { @@ -28,17 +29,74 @@ JoystickExtraControls::JoystickExtraControls() "cmd_null_setpoint", rmw_qos_profile_services_default, callback_group_); // Service cleint to change walking pattern set_gait_pattern_client_ = node_->create_client( - "/gait_generator/set_gait_pattern", rmw_qos_profile_services_default, + "gait_generator/set_gait_pattern", rmw_qos_profile_services_default, callback_group_); + + // Action client to call base twerk action + base_twerk_action_client_ = + rclcpp_action::create_client(node_, "base_twerk_action", + callback_group_); } void JoystickExtraControls::joy_sub_callback( const sensor_msgs::msg::Joy::SharedPtr msg) { - // move base up and down + // move base up and down or yaw control dpad_up_down(msg); // change walking pattern set_gait_pattern(msg); + // change "twerk it!" mode + check_twerk_it_mode_switch(msg); + // check "twerk it!" button + check_twerk_pressed(msg); +} + +void JoystickExtraControls::check_twerk_it_mode_switch( + const sensor_msgs::msg::Joy::SharedPtr msg) { + static int last_button_state = 0; + auto index = twerk_it_params_.get_twerk_it_switch_mode_button_index(); + auto received_button_value = msg->buttons[index]; + + if (received_button_value - last_button_state == 1) { + // Button was pressed + RCLCPP_INFO(node_->get_logger(), + "Button to switch shaking mode pressed, changing mode"); + twerk_it_params_.switch_twerk_it_mode(); + + RCLCPP_INFO(node_->get_logger(), + "Applied twerk mode: %s, magnitude: %f", + twerk_it_params_.get_current_twerk_it_mode_name().c_str(), + twerk_it_params_.get_current_twerk_it_magnitude()); + } + last_button_state = received_button_value; +} + +void JoystickExtraControls::check_twerk_pressed( + const sensor_msgs::msg::Joy::SharedPtr msg) { + + static int last_button_state = 0; + auto index = twerk_it_params_.get_twerk_it_trigger_button_index(); + auto received_button_value = msg->buttons[index]; + + if (received_button_value - last_button_state == 1) { + + if (!is_null_space_motion_possible_) { + RCLCPP_WARN(node_->get_logger(), + "Base motion is not available, skipping base pose change."); + return; + } + + auto twerk_axis_index = twerk_it_params_.get_current_twerk_it_axis(); + auto magnitude = twerk_it_params_.get_current_twerk_it_magnitude(); + auto twerk_time_millis = + twerk_it_params_.get_current_twerk_it_dance_time_millis(); + + this->send_base_twerk_goal(twerk_axis_index, magnitude, twerk_time_millis); + + return; + } + + last_button_state = received_button_value; } void JoystickExtraControls::set_gait_pattern( @@ -92,9 +150,26 @@ void JoystickExtraControls::set_gait_pattern( void JoystickExtraControls::dpad_up_down( const sensor_msgs::msg::Joy::SharedPtr msg) { if (msg->axes[this->d_pad_up_down_axis_index_] != 0) { + + if (!is_null_space_motion_possible_) { + RCLCPP_WARN(node_->get_logger(), + "Base motion is not available, skipping base pose change."); + return; + } + double delta_z = 0.001; double z_disp_base_command = msg->axes[7] * delta_z; + double delta_pitch = 0.01; + double pitch_disp_base_command = msg->axes[7] * delta_pitch; + + if (msg->buttons[5] == 1) { + // If button 5 is pressed, use yaw control instead of z control + z_disp_base_command = 0.0; + } else { + pitch_disp_base_command = 0.0; + } + // Check if the service is available before calling if (!get_current_base_pose_client_->wait_for_service( std::chrono::seconds(100))) { @@ -109,16 +184,17 @@ void JoystickExtraControls::dpad_up_down( // Call the service asynchronously with a callback auto future = get_current_base_pose_client_->async_send_request( request, - [this, z_disp_base_command]( + [this, z_disp_base_command, pitch_disp_base_command]( rclcpp::Client::SharedFuture response) { - this->handle_get_base_pose_response(response, z_disp_base_command); + this->handle_get_base_pose_response(response, z_disp_base_command, + pitch_disp_base_command); }); } } void JoystickExtraControls::handle_get_base_pose_response( rclcpp::Client::SharedFuture response, - double z_disp_base_command) { + double z_disp_base_command, double pitch_disp_base_command) { auto result = response.get(); @@ -154,6 +230,11 @@ void JoystickExtraControls::handle_get_base_pose_response( setpoint_request->pose = result->pose; setpoint_request->pose.pose.position.z += z_disp_base_command; + using namespace penta_pod::kin::commons::quaternion_utils; + auto pitch_quaternion = rpy_to_quaternion(0.0, pitch_disp_base_command, 0.0); // yaw, pitch, roll + setpoint_request->pose.pose.orientation = + hamilton_product(setpoint_request->pose.pose.orientation, pitch_quaternion); + // Call the set_base_pose service asynchronously auto future = set_base_pose_client_->async_send_request( setpoint_request, @@ -174,4 +255,49 @@ void JoystickExtraControls::handle_set_base_pose_response( } } +void JoystickExtraControls::send_base_twerk_goal(int twerk_axis_index, double twerk_magnitude, int twerk_time_millis) { + + if (!this->base_twerk_action_client_->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + return; + } + + auto goal_msg = BaseTwerkAction::Goal(); + + goal_msg.r[twerk_axis_index] = twerk_magnitude; // in meters or rads, depending on axis + goal_msg.w = twerk_it_params_.w; // rad/s + goal_msg.dance_time_millis = twerk_time_millis; // milliseconds + + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(node_->get_logger(), "Goal accepted by server, waiting for result"); + is_null_space_motion_possible_ = false; + } + }; + + send_goal_options.feedback_callback = + [this](const rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr /*feedback*/) { + RCLCPP_INFO(node_->get_logger(), "Received feedback"); + }; + + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { + is_null_space_motion_possible_ = true; + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_INFO(node_->get_logger(), "Goal succeeded"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Goal failed"); + } + }; + + this->base_twerk_action_client_->async_send_goal(goal_msg, send_goal_options); +} + } // namespace penta_pod::teleop::joystick_extra_controls diff --git a/ros_to_ros2_control_command_bridge/CMakeLists.txt b/ros_to_ros2_control_command_bridge/CMakeLists.txt new file mode 100644 index 0000000..8c17d6d --- /dev/null +++ b/ros_to_ros2_control_command_bridge/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.8) +project(ros_to_ros2_control_command_bridge) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +# Declare library +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/forward_joint_command_bridge.cpp + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + sensor_msgs + ) + +add_executable(forward_joint_command_bridge_node src/forward_joint_command_bridge_node.cpp) +target_link_libraries(forward_joint_command_bridge_node + ${PROJECT_NAME} + ) + +############# +# ## Install ## +############# +install( + DIRECTORY include/ + DESTINATION include + ) + +install(TARGETS + forward_joint_command_bridge_node + DESTINATION lib/${PROJECT_NAME} + ) + + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ + ) +############## +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.hpp b/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.hpp new file mode 100644 index 0000000..74345e0 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.hpp @@ -0,0 +1,33 @@ +#ifndef ROS2_CONTROL_FORWARD_JOINT_COMMAND_BRIDGE_HPP +#define ROS2_CONTROL_FORWARD_JOINT_COMMAND_BRIDGE_HPP + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace ros2_to_ros2_control_bridge +{ + class GazeboForwardJointCommandControlBridge + { + + public: + GazeboForwardJointCommandControlBridge(); + ~GazeboForwardJointCommandControlBridge(); + + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg); + void publishJointCommands(); + + void spin() + { + rclcpp::spin(node_); + } + + private: + rclcpp::Node::SharedPtr node_; + + rclcpp::Publisher::SharedPtr joint_command_publisher_; + rclcpp::Subscription::SharedPtr joint_state_subscription_; + }; +} + +#endif // ROS2_CONTROL_FORWARD_JOINT_COMMAND_BRIDGE_HPP \ No newline at end of file diff --git a/ros_to_ros2_control_command_bridge/launch/forward_joint_command_bridge.launch.py b/ros_to_ros2_control_command_bridge/launch/forward_joint_command_bridge.launch.py new file mode 100644 index 0000000..c429b3e --- /dev/null +++ b/ros_to_ros2_control_command_bridge/launch/forward_joint_command_bridge.launch.py @@ -0,0 +1,33 @@ +import launch +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + """ Launch args """ + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + ld.add_action(name_space_arg) + + setpoint_to_frward_joint_position = Node( + package="ros_to_ros2_control_command_bridge", + executable="forward_joint_command_bridge_node", + output="both", + remappings={ + ( + "forward_position_controller/commands", + "/forward_position_controller/commands", + ), # this is for the output topic + ("joint_setpoints", "/joint_setpoints"), + }, # this is for the input topic + ) + ld.add_action(setpoint_to_frward_joint_position) + + return ld diff --git a/ros_to_ros2_control_command_bridge/package.xml b/ros_to_ros2_control_command_bridge/package.xml new file mode 100644 index 0000000..4f8f698 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/package.xml @@ -0,0 +1,22 @@ + + + + ros_to_ros2_control_command_bridge + 0.0.0 + TODO: Package description + modi + TODO: License declaration + + rclcpp + std_msgs + sensor_msgs + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros_to_ros2_control_command_bridge/src/forward_joint_command_bridge_node.cpp b/ros_to_ros2_control_command_bridge/src/forward_joint_command_bridge_node.cpp new file mode 100644 index 0000000..5b0ff74 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/src/forward_joint_command_bridge_node.cpp @@ -0,0 +1,10 @@ +#include "ros_to_ros2_control_command_bridge/forward_joint_command_bridge.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto bridge = std::make_shared(); + bridge->spin(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.cpp b/ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.cpp new file mode 100644 index 0000000..84678aa --- /dev/null +++ b/ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/forward_joint_command_bridge.cpp @@ -0,0 +1,29 @@ +#include "ros_to_ros2_control_command_bridge/forward_joint_command_bridge.hpp" + +namespace ros2_to_ros2_control_bridge +{ +GazeboForwardJointCommandControlBridge::GazeboForwardJointCommandControlBridge() : + node_(rclcpp::Node::make_shared("gazebo_forward_joint_command_control_bridge")) { + joint_command_publisher_ = node_->create_publisher("forward_position_controller/commands", 10); + joint_state_subscription_ = node_->create_subscription( + "joint_setpoints", 10, [this](const sensor_msgs::msg::JointState::SharedPtr msg) { + this->jointStateCallback(msg); + }); +} + +GazeboForwardJointCommandControlBridge::~GazeboForwardJointCommandControlBridge() +{ + // Destructor logic if needed +} + +void GazeboForwardJointCommandControlBridge::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +{ + // Process the joint state message and prepare commands + std_msgs::msg::Float64MultiArray joint_commands; + joint_commands.data = msg->position; // Example: forwarding position as command + + // Publish the joint commands + joint_command_publisher_->publish(joint_commands); +} + +} \ No newline at end of file