From a9b012b92ed4332f8868cfd347867b25dddbff68 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 19 Jul 2025 17:35:19 +0200 Subject: [PATCH 01/40] add name space into xacro and inside nodes --- .../joints_aggregator/joints_aggregator.hpp | 1 + .../launch/joints_aggregator.launch.py | 5 +- .../joints_aggregator/joints_aggregator.cpp | 13 +++- .../launch/robot_state_publisher.launch.py | 43 ++++-------- penta_description/urdf/penta.urdf.xacro | 25 +++---- penta_description/urdf/single_limb.urdf.xacro | 66 +++++++++---------- 6 files changed, 76 insertions(+), 77 deletions(-) diff --git a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp index a812677..215323e 100644 --- a/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp +++ b/joints_aggregator/include/joints_aggregator/joints_aggregator.hpp @@ -31,6 +31,7 @@ class JointsAggregator { std::mutex q_mutex_; std::vector q_; std::vector previous_q_; + std::string name_space_; int limbs_num_; std::vector joints_per_limb_; int update_interval_millis_; diff --git a/joints_aggregator/launch/joints_aggregator.launch.py b/joints_aggregator/launch/joints_aggregator.launch.py index 0467f14..211ef80 100644 --- a/joints_aggregator/launch/joints_aggregator.launch.py +++ b/joints_aggregator/launch/joints_aggregator.launch.py @@ -30,7 +30,10 @@ def generate_launch_description(): executable="joints_aggregator_node", output="screen", namespace=LaunchConfiguration("name_space"), - parameters=[config], + parameters=[ + {"name_space": LaunchConfiguration("name_space")}, + 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 b8a5e7d..772c838 100644 --- a/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp +++ b/joints_aggregator/src/joints_aggregator/joints_aggregator.cpp @@ -24,7 +24,7 @@ JointsAggregator::JointsAggregator() for (int j = 0; j < joints_per_limb_[i]; j++) { joints_count = joints_count + 1; std::string temp = - "limb" + std::to_string(i) + "/joint" + std::to_string(j); + name_space_ + "limb" + std::to_string(i) + "/joint" + std::to_string(j); RCLCPP_INFO(node_->get_logger(), "joint state [%d] name is %s", joints_count, temp.c_str()); joints_states_names.push_back(temp); @@ -100,6 +100,7 @@ JointsAggregator::JointsAggregator() void JointsAggregator::declare_parameters() { // robot geometry + node_->declare_parameter("name_space", ""); node_->declare_parameter("limbs_num"); node_->declare_parameter>("joints_per_limb", std::vector{}); @@ -116,6 +117,16 @@ void JointsAggregator::declare_parameters() { } auto JointsAggregator::get_parameters() -> bool { + // load name_space + if (node_->get_parameter("name_space", name_space_)) { + RCLCPP_INFO_STREAM(node_->get_logger(), + "name_space parameter loaded and equal to: " + << name_space_); + } else { + RCLCPP_ERROR(node_->get_logger(), + "ERROR, can not load name_space parameter"); + return false; + } // load limbs_num if (node_->get_parameter("limbs_num", limbs_num_)) { RCLCPP_INFO_STREAM( diff --git a/penta_description/launch/robot_state_publisher.launch.py b/penta_description/launch/robot_state_publisher.launch.py index 9b62822..04d8106 100644 --- a/penta_description/launch/robot_state_publisher.launch.py +++ b/penta_description/launch/robot_state_publisher.launch.py @@ -1,38 +1,11 @@ from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution 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="", @@ -43,7 +16,17 @@ def generate_launch_description(): default_value='/joint_states', description='Robot state publisher input topic' ) - """ Lauch description """ + + robot_description_content = Command([ + 'xacro ', + PathJoinSubstitution([ + FindPackageShare('penta_description'), + 'urdf', + 'penta.urdf.xacro' + ]), + ' name_space:=', LaunchConfiguration('name_space') + ]) + return LaunchDescription([ name_space_arg, remapping_arg, @@ -54,7 +37,7 @@ def generate_launch_description(): name='robot_state_publisher', output='screen', parameters=[ - {'robot_description': robot_description} + {'robot_description': robot_description_content} ], remappings=[ ('joint_states', LaunchConfiguration('joint_states_remappings')), diff --git a/penta_description/urdf/penta.urdf.xacro b/penta_description/urdf/penta.urdf.xacro index 0c13b13..8ea0558 100644 --- a/penta_description/urdf/penta.urdf.xacro +++ b/penta_description/urdf/penta.urdf.xacro @@ -2,6 +2,7 @@ + @@ -67,8 +68,8 @@ - - + + @@ -77,22 +78,22 @@ - + - + - - - - - + + + + + - - + + @@ -105,7 +106,7 @@ - + diff --git a/penta_description/urdf/single_limb.urdf.xacro b/penta_description/urdf/single_limb.urdf.xacro index 171c57b..bce619b 100644 --- a/penta_description/urdf/single_limb.urdf.xacro +++ b/penta_description/urdf/single_limb.urdf.xacro @@ -2,8 +2,8 @@ - - + + @@ -18,7 +18,7 @@ - + @@ -33,7 +33,7 @@ - + @@ -48,7 +48,7 @@ - + @@ -63,7 +63,7 @@ - + @@ -111,59 +111,59 @@ - + - + - - - + + + - - - + + + - - - + + + - - - + + + - + gazebo_ros2_control/GazeboSystem - + - + - + @@ -171,34 +171,34 @@ - + 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 From 64ad88c1b274feef1a3057e533ab611aa3a9a03e Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 11:25:06 +0200 Subject: [PATCH 02/40] wip --- .../pca9685_hw_interface.hpp | 6 +-- .../hardware/pca9685_hw_interface.cpp | 42 ++++++++++++------- .../ros2_control/pca9685.ros2_control.xacro | 2 + penta_pod/package.xml | 1 + .../forward_joint_command_bridge.launch.py | 2 +- .../forward_joint_command_bridge.cpp | 7 ++-- 6 files changed, 39 insertions(+), 21 deletions(-) diff --git a/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp b/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp index 9ef2e38..80b7368 100644 --- a/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp +++ b/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp @@ -70,9 +70,9 @@ class Pca9685HardwareInterface : public hardware_interface::SystemInterface rclcpp::Clock::SharedPtr clock_; // Store command and joints feedback - std::vector joint_position_commands_; - std::vector joint_positions_; - std::vector joint_velocities_; + std::vector joint_position_commands_rad_; + std::vector joint_positions_rad_; + std::vector joint_velocities_rad_per_sec_; std::vector joint_efforts_; // Motors parameters diff --git a/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp b/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp index 183f885..4f1f70c 100644 --- a/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp +++ b/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp @@ -51,11 +51,13 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_init( auto min_angle = parse_list(min_angle_string); std::string max_angle_string = info_.hardware_parameters.at("motors.max_joint_angle_for_each_channel"); auto max_angle = parse_list(max_angle_string); + auto init_angles = parse_list(info_.hardware_parameters.at("motors.init_angles")); if (min_pulse.size() != static_cast(number_of_motors_) || max_pulse.size() != static_cast(number_of_motors_) || min_angle.size() != static_cast(number_of_motors_) || - max_angle.size() != static_cast(number_of_motors_)) + max_angle.size() != static_cast(number_of_motors_) || + init_angles.size() != static_cast(number_of_motors_)) { RCLCPP_FATAL( get_logger(), "Number of motors (%d) does not match size of pulse or angle parameters.", @@ -76,9 +78,21 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_init( } // Initialize vectors for positions, velocities, efforts and commands - joint_position_commands_ = std::vector(number_of_motors_, 0.); - joint_positions_ = std::vector(number_of_motors_, 0.); - joint_velocities_ = std::vector(number_of_motors_, 0.); + joint_position_commands_rad_ = std::vector(number_of_motors_, 0.); + joint_positions_rad_ = std::vector(number_of_motors_, 0.); + for (int i = 0; i < number_of_motors_; ++i) { + if (init_angles[i] < min_angle[i] || init_angles[i] > max_angle[i]) { + RCLCPP_FATAL( + get_logger(), "Initial angle %ld for motor %d exceeds limits [%ld, %ld].", + init_angles[i], i, min_angle[i], max_angle[i]); + return hardware_interface::CallbackReturn::ERROR; + } else { + auto angle_rad = init_angles[i] * (M_PI / 180.0); // Convert degrees to radians + joint_position_commands_rad_[i] = angle_rad; // Store in radians + joint_positions_rad_[i] = angle_rad; // Initialize positions with initial angles + } + } + joint_velocities_rad_per_sec_ = std::vector(number_of_motors_, 0.); joint_efforts_ = std::vector(number_of_motors_, 0.); for (const hardware_interface::ComponentInfo & joint : info_.joints) @@ -138,10 +152,10 @@ std::vector Pca9685HardwareInterface::export { state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_positions_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_positions_rad_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_velocities_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_velocities_rad_per_sec_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joint_efforts_[i])); @@ -157,7 +171,7 @@ std::vector Pca9685HardwareInterface::expo { command_interfaces.emplace_back( hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_position_commands_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_position_commands_rad_[i])); } return command_interfaces; @@ -211,19 +225,19 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_deactivate( hardware_interface::return_type Pca9685HardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - static std::vector previous_joint_position_commands_; + static std::vector previous_joint_position_commands_rad_; // forward positions for (int index = 0; index < number_of_motors_; ++index) { // read position commands from the motors - joint_positions_[index] = joint_position_commands_[index]; + joint_positions_rad_[index] = joint_position_commands_rad_[index]; } // defferintiate velocities - if (previous_joint_position_commands_.empty()) { - previous_joint_position_commands_ = joint_position_commands_; + if (previous_joint_position_commands_rad_.empty()) { + previous_joint_position_commands_rad_ = joint_position_commands_rad_; } else { for (int index = 0; index < number_of_motors_; ++index) { - joint_velocities_[index] = (joint_position_commands_[index] - previous_joint_position_commands_[index]) / period.seconds(); - previous_joint_position_commands_[index] = joint_position_commands_[index]; + joint_velocities_rad_per_sec_[index] = (joint_position_commands_rad_[index] - previous_joint_position_commands_rad_[index]) / period.seconds(); + previous_joint_position_commands_rad_[index] = joint_position_commands_rad_[index]; } } // efforst stays zero @@ -235,7 +249,7 @@ hardware_interface::return_type Pca9685HardwareInterface::write( { // write position commands to the motors for (int index = 0; index < number_of_motors_; ++index) { - float degree = joint_position_commands_[index]; + float degree = joint_position_commands_rad_[index] * (180.0 / M_PI); // Convert radians to degrees pca_api_->setMotorCommand(index, degree); } pca_api_->flushInternalCommands2Motors(); diff --git a/penta_description/ros2_control/pca9685.ros2_control.xacro b/penta_description/ros2_control/pca9685.ros2_control.xacro index 59b50f1..83302c4 100644 --- a/penta_description/ros2_control/pca9685.ros2_control.xacro +++ b/penta_description/ros2_control/pca9685.ros2_control.xacro @@ -30,6 +30,8 @@ 500, 500, 500, 500, 500, 500, 500, 500] [2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500] + [135.0, 146.0, 87.0, 135.0, 138.0, 87.0, 135.0, 138.0, + 87.0, 135.0, 131.0, 91.0, 135.0, 131.0, 83.0, 90.0] diff --git a/penta_pod/package.xml b/penta_pod/package.xml index fbcbe23..d55fcfe 100644 --- a/penta_pod/package.xml +++ b/penta_pod/package.xml @@ -14,6 +14,7 @@ base_twerk penta_teleop ros_to_ros2_control_command_bridge + pca9685_ros2_control penta_i2c_actuators 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 index 5b412d6..4085fdf 100644 --- 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 @@ -28,7 +28,7 @@ def generate_launch_description(): output="both", remappings={ ( - "forward_position_controller/commands", + "actuator_setpoints", "/forward_position_controller/commands", ), # this is for the output topic ("joint_setpoints", "/joint_setpoints"), 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 index 211e1a7..75c67f2 100644 --- 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 @@ -9,7 +9,7 @@ GazeboForwardJointCommandControlBridge::GazeboForwardJointCommandControlBridge() declareParameters(); loadParameters(); // Create the publisher and subscription - joint_command_publisher_ = node_->create_publisher("forward_position_controller/commands", 10); + joint_command_publisher_ = node_->create_publisher("actuator_setpoints", 10); joint_state_subscription_ = node_->create_subscription( "joint_setpoints", 10, [this](const sensor_msgs::msg::JointState::SharedPtr msg) { this->jointStateCallback(msg); @@ -30,8 +30,9 @@ void GazeboForwardJointCommandControlBridge::jointStateCallback(const sensor_msg for (size_t i = 0; i < n; ++i) { auto q_i_degree = msg->position[i] * 180 / M_PI; auto dq_i_degree = q_i_degree - joint_angles_at_initial_pose_degree_[i]; - auto servo_setpoint = actuator_angles_at_initial_pose_degree_[i] + dir_[i] * dq_i_degree; - joint_commands.data[i] = servo_setpoint; + auto servo_setpoint_degree = actuator_angles_at_initial_pose_degree_[i] + dir_[i] * dq_i_degree; + auto servo_setpoint_rad = servo_setpoint_degree * (M_PI / 180.0); // Convert degrees to radians + joint_commands.data[i] = servo_setpoint_rad; } // Publish the joint commands joint_command_publisher_->publish(joint_commands); From 707b73075d98b0b5ae32227a13a0a592a2772fe0 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:00:53 +0200 Subject: [PATCH 03/40] fix naming, the hardware interface deals with the acutator setpoint not the joint set point --- .../include/pca9685_ros2_control/pca9685_hw_interface.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp b/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp index 80b7368..4b60ec3 100644 --- a/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp +++ b/pac9685_cpp/pca9685_ros2_control/hardware/include/pca9685_ros2_control/pca9685_hw_interface.hpp @@ -70,10 +70,10 @@ class Pca9685HardwareInterface : public hardware_interface::SystemInterface rclcpp::Clock::SharedPtr clock_; // Store command and joints feedback - std::vector joint_position_commands_rad_; - std::vector joint_positions_rad_; - std::vector joint_velocities_rad_per_sec_; - std::vector joint_efforts_; + std::vector actuator_position_commands_rad_; + std::vector actuator_positions_feedback_rad_; + std::vector actuator_velocities_rad_per_sec_; + std::vector actuator_efforts_; // Motors parameters std::string i2c_device_name_; From e28feb78773ee29a2ed06403b88da23edd73219e Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:01:10 +0200 Subject: [PATCH 04/40] fix naming, the hardware interface deals with the acutator setpoint not the joint set point --- .../hardware/pca9685_hw_interface.cpp | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp b/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp index 4f1f70c..c43a60b 100644 --- a/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp +++ b/pac9685_cpp/pca9685_ros2_control/hardware/pca9685_hw_interface.cpp @@ -78,8 +78,8 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_init( } // Initialize vectors for positions, velocities, efforts and commands - joint_position_commands_rad_ = std::vector(number_of_motors_, 0.); - joint_positions_rad_ = std::vector(number_of_motors_, 0.); + actuator_position_commands_rad_ = std::vector(number_of_motors_, 0.); + actuator_positions_feedback_rad_ = std::vector(number_of_motors_, 0.); for (int i = 0; i < number_of_motors_; ++i) { if (init_angles[i] < min_angle[i] || init_angles[i] > max_angle[i]) { RCLCPP_FATAL( @@ -88,12 +88,12 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } else { auto angle_rad = init_angles[i] * (M_PI / 180.0); // Convert degrees to radians - joint_position_commands_rad_[i] = angle_rad; // Store in radians - joint_positions_rad_[i] = angle_rad; // Initialize positions with initial angles + actuator_position_commands_rad_[i] = angle_rad; // Store in radians + actuator_positions_feedback_rad_[i] = angle_rad; // Initialize positions with initial angles } } - joint_velocities_rad_per_sec_ = std::vector(number_of_motors_, 0.); - joint_efforts_ = std::vector(number_of_motors_, 0.); + actuator_velocities_rad_per_sec_ = std::vector(number_of_motors_, 0.); + actuator_efforts_ = std::vector(number_of_motors_, 0.); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -152,13 +152,13 @@ std::vector Pca9685HardwareInterface::export { state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_positions_rad_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &actuator_positions_feedback_rad_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_velocities_rad_per_sec_[i])); + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &actuator_velocities_rad_per_sec_[i])); state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joint_efforts_[i])); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &actuator_efforts_[i])); } return state_interfaces; @@ -171,7 +171,7 @@ std::vector Pca9685HardwareInterface::expo { command_interfaces.emplace_back( hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_position_commands_rad_[i])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &actuator_position_commands_rad_[i])); } return command_interfaces; @@ -225,19 +225,19 @@ hardware_interface::CallbackReturn Pca9685HardwareInterface::on_deactivate( hardware_interface::return_type Pca9685HardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - static std::vector previous_joint_position_commands_rad_; + static std::vector previous_actuator_position_commands_rad_; // forward positions for (int index = 0; index < number_of_motors_; ++index) { // read position commands from the motors - joint_positions_rad_[index] = joint_position_commands_rad_[index]; + actuator_positions_feedback_rad_[index] = actuator_position_commands_rad_[index]; } // defferintiate velocities - if (previous_joint_position_commands_rad_.empty()) { - previous_joint_position_commands_rad_ = joint_position_commands_rad_; + if (previous_actuator_position_commands_rad_.empty()) { + previous_actuator_position_commands_rad_ = actuator_position_commands_rad_; } else { for (int index = 0; index < number_of_motors_; ++index) { - joint_velocities_rad_per_sec_[index] = (joint_position_commands_rad_[index] - previous_joint_position_commands_rad_[index]) / period.seconds(); - previous_joint_position_commands_rad_[index] = joint_position_commands_rad_[index]; + actuator_velocities_rad_per_sec_[index] = (actuator_position_commands_rad_[index] - previous_actuator_position_commands_rad_[index]) / period.seconds(); + previous_actuator_position_commands_rad_[index] = actuator_position_commands_rad_[index]; } } // efforst stays zero @@ -249,7 +249,7 @@ hardware_interface::return_type Pca9685HardwareInterface::write( { // write position commands to the motors for (int index = 0; index < number_of_motors_; ++index) { - float degree = joint_position_commands_rad_[index] * (180.0 / M_PI); // Convert radians to degrees + float degree = actuator_position_commands_rad_[index] * (180.0 / M_PI); // Convert radians to degrees pca_api_->setMotorCommand(index, degree); } pca_api_->flushInternalCommands2Motors(); From e7cfe78e8d1ef9e893eeeaa8443284536ee54f46 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:02:26 +0200 Subject: [PATCH 05/40] wip, adding name space --- .../ros2_control_pca9685/ros2_control_pca9685.launch.py | 9 ++++++--- .../launch/forward_joint_command_bridge.launch.py | 5 +++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py b/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py index 996f498..2e5cd19 100644 --- a/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py +++ b/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py @@ -76,8 +76,10 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[controller_manager_config], + namespace=LaunchConfiguration("name_space"), remappings=[ - ("/controller_manager/robot_description", "/robot_description"), + ("controller_manager/robot_description", "robot_description"), + ("joint_states", "actuator_states"), ], output="both", ) @@ -85,10 +87,11 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", + namespace=LaunchConfiguration("name_space"), arguments=[ "joint_state_broadcaster", "--controller-manager", - "/controller_manager", + "controller_manager", ], output="screen", ) @@ -99,7 +102,7 @@ def generate_launch_description(): arguments=[ "forward_position_controller", "--controller-manager", - "/controller_manager", + "controller_manager", ], output="screen", ) 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 index 4085fdf..3435a32 100644 --- 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 @@ -25,13 +25,14 @@ def generate_launch_description(): setpoint_to_frward_joint_position = Node( package="ros_to_ros2_control_command_bridge", executable="forward_joint_command_bridge_node", + namespace=launch.substitutions.LaunchConfiguration("name_space"), output="both", remappings={ ( "actuator_setpoints", - "/forward_position_controller/commands", + "forward_position_controller/commands", ), # this is for the output topic - ("joint_setpoints", "/joint_setpoints"), + ("joint_setpoints", "joint_setpoints"), }, # this is for the input topic parameters=[ {"name_space": launch.substitutions.LaunchConfiguration("name_space")}, From dd942112a45ffffbe18d5cf866b287382f19ab27 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:17:30 +0200 Subject: [PATCH 06/40] add name space to link names --- .../gait_generator/base_tf_broadcaster.hpp | 4 ++++ .../src/gait_generator/base_tf_broadcaster.cpp | 15 +++++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/gait_generator/include/gait_generator/base_tf_broadcaster.hpp b/gait_generator/include/gait_generator/base_tf_broadcaster.hpp index 7015fcd..41e5161 100644 --- a/gait_generator/include/gait_generator/base_tf_broadcaster.hpp +++ b/gait_generator/include/gait_generator/base_tf_broadcaster.hpp @@ -19,6 +19,10 @@ class BaseTfBroadcaster { rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr base_pose_subscriber_; auto base_pose_sub_callback(const PoseStamped::SharedPtr msg) -> void; + std::string name_space_; + + void declare_parameters(); + void get_parameters(); public: explicit BaseTfBroadcaster(); diff --git a/gait_generator/src/gait_generator/base_tf_broadcaster.cpp b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp index 819ee20..060e5df 100644 --- a/gait_generator/src/gait_generator/base_tf_broadcaster.cpp +++ b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp @@ -18,8 +18,8 @@ auto BaseTfBroadcaster::base_pose_sub_callback(const PoseStamped::SharedPtr msg) geometry_msgs::msg::TransformStamped transformStamped; // header transformStamped.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); - transformStamped.header.frame_id = "base_footprint"; - transformStamped.child_frame_id = "base_link"; + transformStamped.header.frame_id = name_space_ + "base_footprint"; + transformStamped.child_frame_id = name_space_ + "base_link"; // translation transformStamped.transform.translation.x = msg->pose.position.x; transformStamped.transform.translation.y = msg->pose.position.y; @@ -34,4 +34,15 @@ auto BaseTfBroadcaster::base_pose_sub_callback(const PoseStamped::SharedPtr msg) br->sendTransform(transformStamped); } +auto BaseTfBroadcaster::declare_parameters() -> void { + node_->declare_parameter("name_space", ""); +} + +auto BaseTfBroadcaster::get_parameters() -> void { + node_->get_parameter("name_space", name_space_); + RCLCPP_INFO(node_->get_logger(), + "BaseTfBroadcaster: name_space: %s", + name_space_.c_str()); +} + } // namespace penta_pod::kin \ No newline at end of file From aeb43b5601697f28cd12cf49764cc94587771754 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:47:36 +0200 Subject: [PATCH 07/40] add name spaces to allow simulating several robots with different name space --- .../launch/gait_generator.launch.py | 5 +++- penta_description/urdf/penta.urdf.xacro | 26 +++++++++++-------- penta_description/urdf/single_limb.urdf.xacro | 2 +- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/gait_generator/launch/gait_generator.launch.py b/gait_generator/launch/gait_generator.launch.py index 7978d85..a6a6b8d 100644 --- a/gait_generator/launch/gait_generator.launch.py +++ b/gait_generator/launch/gait_generator.launch.py @@ -44,12 +44,15 @@ def generate_launch_description(): package="gait_generator", executable="base_tf_broadcaster_node", output="screen", - parameters=[config], namespace=LaunchConfiguration("name_space"), remappings=[ # output topics ("null_space_pose", "null_space_pose"), ], + parameters=[ + {"name_space": LaunchConfiguration("name_space")}, + config + ], ) ) return ld diff --git a/penta_description/urdf/penta.urdf.xacro b/penta_description/urdf/penta.urdf.xacro index 8ea0558..d6b802f 100644 --- a/penta_description/urdf/penta.urdf.xacro +++ b/penta_description/urdf/penta.urdf.xacro @@ -24,9 +24,11 @@ - + - + + + @@ -47,13 +49,13 @@ - - - + + + - + @@ -62,12 +64,14 @@ - - - + + + + + @@ -79,13 +83,13 @@ - + - + diff --git a/penta_description/urdf/single_limb.urdf.xacro b/penta_description/urdf/single_limb.urdf.xacro index bce619b..5ccbf37 100644 --- a/penta_description/urdf/single_limb.urdf.xacro +++ b/penta_description/urdf/single_limb.urdf.xacro @@ -112,7 +112,7 @@ - + From de3aba78246bfe5fbee07a3bef82a606afbc95f9 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 12:55:01 +0200 Subject: [PATCH 08/40] add missing declare and load parameters --- gait_generator/src/gait_generator/base_tf_broadcaster.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gait_generator/src/gait_generator/base_tf_broadcaster.cpp b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp index 060e5df..6779813 100644 --- a/gait_generator/src/gait_generator/base_tf_broadcaster.cpp +++ b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp @@ -7,6 +7,10 @@ BaseTfBroadcaster::BaseTfBroadcaster() RCLCPP_INFO(node_->get_logger(), "Starting base_tf_broadcaster, subscriping to null_space_pose " "topic and broadcasting tf"); + // declare and get parameters + declare_parameters(); + get_parameters(); + // create subscriber to null_space_pose topic base_pose_subscriber_ = node_->create_subscription( "null_space_pose", 10, [this](const PoseStamped::SharedPtr msg) { this->base_pose_sub_callback(msg); From 2787c745e3a43bb30ef24481e39acbd691b66532 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 13:26:47 +0200 Subject: [PATCH 09/40] add missing namespace --- .../launch/ros2_control_pca9685/ros2_control_pca9685.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py b/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py index 2e5cd19..aece18f 100644 --- a/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py +++ b/penta_pod/launch/ros2_control_pca9685/ros2_control_pca9685.launch.py @@ -99,6 +99,7 @@ def generate_launch_description(): forward_controller_spawner = Node( package="controller_manager", executable="spawner", + namespace=LaunchConfiguration("name_space"), arguments=[ "forward_position_controller", "--controller-manager", From 51b712992f20d884507a158627cc2c24af4588f9 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 20 Jul 2025 13:41:49 +0200 Subject: [PATCH 10/40] update readme --- README.md | 14 +++++++++++++- penta_pod/README.md | 6 ++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 46da11f..1de9023 100644 --- a/README.md +++ b/README.md @@ -202,4 +202,16 @@ You can check that the topics are in the namespace, same for services and action /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 +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 + + + +# ROS2 control + +HW interface for PCA9685 is created, and can be used to control the robot with hoppy servo motors + +## To bring up real hardware using ros2_control + +``` +ros2 launch penta_pod ros2_control_penta.launch.py +``` \ No newline at end of file diff --git a/penta_pod/README.md b/penta_pod/README.md index 4f33238..6e7a7a6 100644 --- a/penta_pod/README.md +++ b/penta_pod/README.md @@ -15,3 +15,9 @@ ros2 launch penta_pod penta_sim_full_rviz.launch.py ``` ros2 launch penta_pod realhardware_bringup.launch.py ``` + +## To bring up real hardware using ros2_control + +``` +ros2 launch penta_pod ros2_control_penta.launch.py +``` \ No newline at end of file From a0559996fba5a0cd886f26d812e63cf6ad5ed495 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:21:52 +0200 Subject: [PATCH 11/40] pass gazebo arg into launch file and xacro command --- .../launch/robot_state_publisher.launch.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/penta_description/launch/robot_state_publisher.launch.py b/penta_description/launch/robot_state_publisher.launch.py index 04d8106..416bce3 100644 --- a/penta_description/launch/robot_state_publisher.launch.py +++ b/penta_description/launch/robot_state_publisher.launch.py @@ -11,6 +11,11 @@ def generate_launch_description(): default_value="", description="Robot name space", ) + use_gazebo_simulation_arg = DeclareLaunchArgument( + "use_gazebo_simulation", + default_value="False", + description="Change to true to use Gazebo", + ) remapping_arg = DeclareLaunchArgument( 'joint_states_remappings', default_value='/joint_states', @@ -24,11 +29,15 @@ def generate_launch_description(): 'urdf', 'penta.urdf.xacro' ]), - ' name_space:=', LaunchConfiguration('name_space') + ' name_space:=', + LaunchConfiguration('name_space'), + ' use_gazebo_simulation:=', + LaunchConfiguration('use_gazebo_simulation'), ]) return LaunchDescription([ name_space_arg, + use_gazebo_simulation_arg, remapping_arg, Node( package='robot_state_publisher', From e182de6d44f6673878fca0050f905e63bdfcbb77 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:23:54 +0200 Subject: [PATCH 12/40] pass gazebo arg and use right launch file for the bridge --- .../penta_gazebo_sim/launch/penta_sim_full_gazebo.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 index 47a9f8f..0f0f404 100644 --- 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 @@ -36,6 +36,7 @@ def generate_launch_description(): ), launch_arguments={ "name_space": LaunchConfiguration("name_space"), + "use_gazebo_simulation": "True", }.items(), ) ld.add_action(penta_rviz_sim_full) @@ -135,7 +136,7 @@ def generate_launch_description(): os.path.join( get_package_share_directory("ros_to_ros2_control_command_bridge"), "launch", - "forward_joint_command_bridge.launch.py", + "gazebo_forward_joint_command_bridge.launch.py", ) ), launch_arguments={ From 53d9a98752d76c16aa473ed39b6a622de8fd02f3 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:24:47 +0200 Subject: [PATCH 13/40] allow to use gazebo simulation --- penta_pod/launch/penta_rviz.launch.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/penta_pod/launch/penta_rviz.launch.py b/penta_pod/launch/penta_rviz.launch.py index ef86105..d59c0e0 100644 --- a/penta_pod/launch/penta_rviz.launch.py +++ b/penta_pod/launch/penta_rviz.launch.py @@ -21,6 +21,12 @@ def generate_launch_description(): description="Robot name space", ) + use_gazebo_simulation_arg = DeclareLaunchArgument( + "use_gazebo_simulation", + default_value="False", + description="Change to true to use Gazebo", + ) + robot_state_publisher_remapping_arg = DeclareLaunchArgument( 'joint_states_remappings', default_value='/joint_states', @@ -44,11 +50,13 @@ def generate_launch_description(): ), launch_arguments={ 'name_space': LaunchConfiguration('name_space'), + 'use_gazebo_simulation': LaunchConfiguration('use_gazebo_simulation'), 'joint_states_remappings': LaunchConfiguration('joint_states_remappings'), }.items() ) return LaunchDescription([ name_space_arg, + use_gazebo_simulation_arg, robot_state_publisher_remapping_arg, rviz_config_file_arg, Node( From 98b136611342e2385690346be698eecbfce29889 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:29:29 +0200 Subject: [PATCH 14/40] compile ros2 to gazebo ros2 control bridge --- ros_to_ros2_control_command_bridge/CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ros_to_ros2_control_command_bridge/CMakeLists.txt b/ros_to_ros2_control_command_bridge/CMakeLists.txt index 8c17d6d..a9faf96 100644 --- a/ros_to_ros2_control_command_bridge/CMakeLists.txt +++ b/ros_to_ros2_control_command_bridge/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(sensor_msgs REQUIRED) # Declare library add_library(${PROJECT_NAME} src/${PROJECT_NAME}/forward_joint_command_bridge.cpp + src/${PROJECT_NAME}/gazebo_forward_joint_command_bridge.cpp ) target_include_directories(${PROJECT_NAME} @@ -32,6 +33,11 @@ target_link_libraries(forward_joint_command_bridge_node ${PROJECT_NAME} ) +add_executable(gazebo_forward_joint_command_bridge_node src/gazebo_forward_joint_command_bridge_node.cpp) +target_link_libraries(gazebo_forward_joint_command_bridge_node + ${PROJECT_NAME} + ) + ############# # ## Install ## ############# @@ -42,6 +48,7 @@ install( install(TARGETS forward_joint_command_bridge_node + gazebo_forward_joint_command_bridge_node DESTINATION lib/${PROJECT_NAME} ) From 06e872294a364947ae382613687ecf56876b89cd Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:30:56 +0200 Subject: [PATCH 15/40] fix naming --- .../forward_joint_command_bridge.hpp | 6 +++--- .../src/forward_joint_command_bridge_node.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index 98e57d1..3375779 100644 --- 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 @@ -7,12 +7,12 @@ namespace ros2_to_ros2_control_bridge { - class GazeboForwardJointCommandControlBridge + class ForwardJointCommandControlBridge { public: - GazeboForwardJointCommandControlBridge(); - ~GazeboForwardJointCommandControlBridge(); + ForwardJointCommandControlBridge(); + ~ForwardJointCommandControlBridge(); void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg); void publishJointCommands(); 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 index 5b0ff74..6d68f84 100644 --- 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 @@ -3,7 +3,7 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto bridge = std::make_shared(); + auto bridge = std::make_shared(); bridge->spin(); rclcpp::shutdown(); return 0; From 61b962e58e8ab5f8ccb8d40f5b00c8424304b445 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:34:46 +0200 Subject: [PATCH 16/40] fix naming --- .../forward_joint_command_bridge.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 index 75c67f2..843f1c9 100644 --- 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 @@ -2,10 +2,10 @@ namespace ros2_to_ros2_control_bridge { -GazeboForwardJointCommandControlBridge::GazeboForwardJointCommandControlBridge() : - node_(rclcpp::Node::make_shared("gazebo_forward_joint_command_control_bridge")) { +ForwardJointCommandControlBridge::ForwardJointCommandControlBridge() : + node_(rclcpp::Node::make_shared("forward_joint_command_control_bridge")) { // Initialize the node and declare parameters - RCLCPP_INFO(node_->get_logger(), "GazeboForwardJointCommandControlBridge initialized."); + RCLCPP_INFO(node_->get_logger(), "ForwardJointCommandControlBridge initialized."); declareParameters(); loadParameters(); // Create the publisher and subscription @@ -16,12 +16,12 @@ GazeboForwardJointCommandControlBridge::GazeboForwardJointCommandControlBridge() }); } -GazeboForwardJointCommandControlBridge::~GazeboForwardJointCommandControlBridge() +ForwardJointCommandControlBridge::~ForwardJointCommandControlBridge() { // Destructor logic if needed } -void GazeboForwardJointCommandControlBridge::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +void ForwardJointCommandControlBridge::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { // Process the joint state message and prepare commands std_msgs::msg::Float64MultiArray joint_commands; @@ -38,7 +38,7 @@ void GazeboForwardJointCommandControlBridge::jointStateCallback(const sensor_msg joint_command_publisher_->publish(joint_commands); } -void GazeboForwardJointCommandControlBridge::declareParameters() +void ForwardJointCommandControlBridge::declareParameters() { node_->declare_parameter>("joint_angles_at_initial_pose_degree", std::vector{} @@ -52,7 +52,7 @@ void GazeboForwardJointCommandControlBridge::declareParameters() ); } -void GazeboForwardJointCommandControlBridge::loadParameters() +void ForwardJointCommandControlBridge::loadParameters() { auto load_vector_parameter = [this](const std::string ¶m_name, std::vector ¶m_vector) { if (!node_->get_parameter(param_name, param_vector)) { From f32a49631e44b67f4cdf0f5810bd0e6e907916c3 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:39:00 +0200 Subject: [PATCH 17/40] pass gazebo arg --- penta_pod/launch/penta_sim_full_rviz.launch.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/penta_pod/launch/penta_sim_full_rviz.launch.py b/penta_pod/launch/penta_sim_full_rviz.launch.py index cf56d3c..1264650 100644 --- a/penta_pod/launch/penta_sim_full_rviz.launch.py +++ b/penta_pod/launch/penta_sim_full_rviz.launch.py @@ -15,15 +15,18 @@ 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) + + use_gazebo_simulation_arg = DeclareLaunchArgument( + "use_gazebo_simulation", + default_value="False", + description="Change to true to use Gazebo", + ) """ Nodes """ # Include RVIZ launch file @@ -33,6 +36,7 @@ def generate_launch_description(): ), launch_arguments={ 'name_space': LaunchConfiguration('name_space'), + 'use_gazebo_simulation': LaunchConfiguration('use_gazebo_simulation'), 'joint_states_remappings': 'joint_setpoints', }.items() ) @@ -44,12 +48,14 @@ def generate_launch_description(): ), launch_arguments={ 'name_space': LaunchConfiguration('name_space'), + 'use_gazebo_simulation': LaunchConfiguration('use_gazebo_simulation') }.items() ) return LaunchDescription([ # scorption_cmd_vel_mux_launch, name_space_arg, + use_gazebo_simulation_arg, rviz_penta_pod_launch, penta_core_launch, ]) From d06357bcfbf5bd38d815f19f06a0cf6b089516e6 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:40:53 +0200 Subject: [PATCH 18/40] gazebo gazebo ros2 control xacro --- .../ros2_control/gazebo.ros2_control.xacro | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 penta_description/ros2_control/gazebo.ros2_control.xacro diff --git a/penta_description/ros2_control/gazebo.ros2_control.xacro b/penta_description/ros2_control/gazebo.ros2_control.xacro new file mode 100644 index 0000000..6027dde --- /dev/null +++ b/penta_description/ros2_control/gazebo.ros2_control.xacro @@ -0,0 +1,17 @@ + + + + + + + $(find penta_description)/config/ros2_control_params.yaml + + + /robot_description:='${name_space}'/robot_description + + + + + + + From ba727f12e83a57b27ce5fcdc97a57c4cc2a30b0a Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 22 Jul 2025 00:43:57 +0200 Subject: [PATCH 19/40] ros2 to gazebo simulation --- .../gazebo_forward_joint_command_bridge.hpp | 34 ++++++++++++++++++ ...ebo_forward_joint_command_bridge.launch.py | 35 +++++++++++++++++++ ...zebo_forward_joint_command_bridge_node.cpp | 10 ++++++ .../gazebo_forward_joint_command_bridge.cpp | 33 +++++++++++++++++ 4 files changed, 112 insertions(+) create mode 100644 ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.hpp create mode 100644 ros_to_ros2_control_command_bridge/launch/gazebo_forward_joint_command_bridge.launch.py create mode 100644 ros_to_ros2_control_command_bridge/src/gazebo_forward_joint_command_bridge_node.cpp create mode 100644 ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.cpp diff --git a/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.hpp b/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.hpp new file mode 100644 index 0000000..5e23372 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/include/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.hpp @@ -0,0 +1,34 @@ +#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 gazebo_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/gazebo_forward_joint_command_bridge.launch.py b/ros_to_ros2_control_command_bridge/launch/gazebo_forward_joint_command_bridge.launch.py new file mode 100644 index 0000000..30119de --- /dev/null +++ b/ros_to_ros2_control_command_bridge/launch/gazebo_forward_joint_command_bridge.launch.py @@ -0,0 +1,35 @@ +import launch +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory +import os + +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="gazebo_forward_joint_command_bridge_node", + namespace=launch.substitutions.LaunchConfiguration("name_space"), + 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/src/gazebo_forward_joint_command_bridge_node.cpp b/ros_to_ros2_control_command_bridge/src/gazebo_forward_joint_command_bridge_node.cpp new file mode 100644 index 0000000..fbf9fa8 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/src/gazebo_forward_joint_command_bridge_node.cpp @@ -0,0 +1,10 @@ +#include "ros_to_ros2_control_command_bridge/gazebo_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/gazebo_forward_joint_command_bridge.cpp b/ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.cpp new file mode 100644 index 0000000..3147034 --- /dev/null +++ b/ros_to_ros2_control_command_bridge/src/ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.cpp @@ -0,0 +1,33 @@ +#include "ros_to_ros2_control_command_bridge/gazebo_forward_joint_command_bridge.hpp" + +namespace gazebo_ros2_to_ros2_control_bridge +{ +GazeboForwardJointCommandControlBridge::GazeboForwardJointCommandControlBridge() : + node_(rclcpp::Node::make_shared("gazebo_forward_joint_command_control_bridge")) { + // Initialize the node and declare parameters + RCLCPP_INFO(node_->get_logger(), "GazeboForwardJointCommandControlBridge initialized."); + + // Create the publisher and subscription + 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 From 411fe1eea783281bfbff8423a60cc355cdd0dd0d Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 20:43:06 +0200 Subject: [PATCH 20/40] odometry publisher --- cmd_vel_to_odom/CMakeLists.txt | 76 +++++++++++++ .../cmd_vel_to_odom/cmd_vel_to_odom.hpp | 38 +++++++ .../launch/cmd_vel_to_odom.launch.py | 29 +++++ cmd_vel_to_odom/package.xml | 26 +++++ .../src/cmd_vel_to_odom/cmd_vel_to_odom.cpp | 107 ++++++++++++++++++ cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp | 8 ++ 6 files changed, 284 insertions(+) create mode 100644 cmd_vel_to_odom/CMakeLists.txt create mode 100644 cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp create mode 100644 cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py create mode 100644 cmd_vel_to_odom/package.xml create mode 100644 cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp create mode 100644 cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp diff --git a/cmd_vel_to_odom/CMakeLists.txt b/cmd_vel_to_odom/CMakeLists.txt new file mode 100644 index 0000000..946ae22 --- /dev/null +++ b/cmd_vel_to_odom/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.12) +project(cmd_vel_to_odom) + +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Werror -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +# # Declare battery system driver library +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/${PROJECT_NAME}.cpp + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) + + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + ) + + +add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ) + + +############# +## Install ## +############# + +install( + DIRECTORY include/ + DESTINATION include + ) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} + ) + +##############. +## Testing ## +############## + +if(BUILD_TESTING) +endif() + +ament_package() diff --git a/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp b/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp new file mode 100644 index 0000000..787c263 --- /dev/null +++ b/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp @@ -0,0 +1,38 @@ +#ifndef POLYPOD_CMD_VEL_TO_ODOM_HPP_ +#define POLYPOD_CMD_VEL_TO_ODOM_HPP_ + +#include +#include "rclcpp/rclcpp.hpp" // for rclcpp + +// include messages +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" +#include "geometry_msgs/msg/twist_with_covariance.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +namespace polypod::cmd_vel_to_odom { + class CmdVelToOdom { + public: + explicit CmdVelToOdom(); + void spin() { rclcpp::spin(node_); } + + private: + rclcpp::Node::SharedPtr node_; + + double theta_{}; + + geometry_msgs::msg::Twist cmd_vel_{}; + nav_msgs::msg::Odometry state_{}; + + rclcpp::Subscription::SharedPtr cmd_vel_subscriber_; + rclcpp::Publisher::SharedPtr odometry_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + void declare_parameters(); + }; + +} // namespace polypod::cmd_vel_to_odom + +#endif // POLYPOD_CMD_VEL_TO_ODOM_HPP_ \ No newline at end of file diff --git a/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py b/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py new file mode 100644 index 0000000..8ace09e --- /dev/null +++ b/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py @@ -0,0 +1,29 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Namespace for the node" + ) + + return LaunchDescription( + [ + name_space_arg, + Node( + package="cmd_vel_to_odom", + executable="cmd_vel_to_odom_node", + namespace=LaunchConfiguration("name_space"), + output="screen", + remappings=[ + ("feedback_cmd_vel", "feedback_cmd_vel"), + ("odom", "odom"), + ], + ), + ] + ) diff --git a/cmd_vel_to_odom/package.xml b/cmd_vel_to_odom/package.xml new file mode 100644 index 0000000..254547e --- /dev/null +++ b/cmd_vel_to_odom/package.xml @@ -0,0 +1,26 @@ + + + + cmd_vel_to_odom + 0.0.0 + TODO: Package description + mohammad.safeea + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + nav_msgs + tf2_ros + launch_testing + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp b/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp new file mode 100644 index 0000000..f1c4514 --- /dev/null +++ b/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp @@ -0,0 +1,107 @@ +#include + +// include librarries +#include "cmd_vel_to_odom/cmd_vel_to_odom.hpp" +#include "rclcpp/rclcpp.hpp" + + +// include messages +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" +#include "geometry_msgs/msg/twist_with_covariance.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include + +#define pi 3.141592 + +namespace polypod::cmd_vel_to_odom { + + CmdVelToOdom::CmdVelToOdom() : node_{rclcpp::Node::make_shared("cmd_vel_to_odom_node")} { + RCLCPP_INFO(node_->get_logger(), "Starting cmd_vel to odom node"); + + odometry_publisher_ = node_->create_publisher("odom", 10); + + cmd_vel_subscriber_ = node_->create_subscription( + "feedback_cmd_vel", 1, + [this](const geometry_msgs::msg::Twist& cmd_vel_msg) -> void { + cmd_vel_ = cmd_vel_msg; + }); + + constexpr auto timer_period_millis = 100; + timer_ = node_->create_wall_timer( + std::chrono::milliseconds(timer_period_millis), + [this, timer_period_millis]() -> void { + + auto vx = cmd_vel_.linear.x; + auto vy = cmd_vel_.linear.y; + auto w = cmd_vel_.angular.z; + + constexpr auto millis_to_seconds = 1000.0; + auto vx_dt = vx * (timer_period_millis / millis_to_seconds); + auto vy_dt = vy * (timer_period_millis / millis_to_seconds); + auto w_dt = w * (timer_period_millis / millis_to_seconds); + theta_ += w_dt; + + auto dx = vx_dt * cos(theta_) - vy_dt * sin(theta_); + auto dy = vx_dt * sin(theta_) + vy_dt * cos(theta_); + + state_.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + state_.header.frame_id = "odom"; + state_.child_frame_id = "base_footprint"; + state_.pose.pose.position.x += dx; + state_.pose.pose.position.y += dy; + state_.pose.pose.position.z = 0.0; + state_.pose.pose.orientation.x = 0.0; + state_.pose.pose.orientation.y = 0.0; + state_.pose.pose.orientation.z = sin(theta_/2); + state_.pose.pose.orientation.w = cos(theta_/2); + + // Populate the twist information + state_.twist.twist.linear.x = vx; + state_.twist.twist.linear.y = vy; + state_.twist.twist.angular.z = w; + + state_.pose.covariance = {0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, + 0, 0, 0.01, 0, 0, 0, + 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0.01}; + + state_.twist.covariance = {0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, + 0, 0, 0.01, 0, 0, 0, + 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0.01}; + + + odometry_publisher_->publish(state_); + + // publish transforms + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = state_.header.stamp; + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "base_footprint"; + + // Set the translation + transformStamped.transform.translation.x = state_.pose.pose.position.x; + transformStamped.transform.translation.y = state_.pose.pose.position.y; + transformStamped.transform.translation.z = state_.pose.pose.position.z; + transformStamped.transform.rotation.x = state_.pose.pose.orientation.x; + transformStamped.transform.rotation.y = state_.pose.pose.orientation.y; + transformStamped.transform.rotation.z = state_.pose.pose.orientation.z; + transformStamped.transform.rotation.w = state_.pose.pose.orientation.w; + // Broadcast the transform + // tf_broadcaster_->publish(transformStamped); + // tf2_ros + static auto br = std::make_shared(node_); + br->sendTransform(transformStamped); + } + ); + } + +} // namespace polypod::cmd_vel_to_odom diff --git a/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp b/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp new file mode 100644 index 0000000..1c12548 --- /dev/null +++ b/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp @@ -0,0 +1,8 @@ +#include "cmd_vel_to_odom/cmd_vel_to_odom.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + polypod::cmd_vel_to_odom::CmdVelToOdom().spin(); + rclcpp::shutdown(); + return 0; +} From 4556a99cbc11c413f73a0ad9f1f0be8b86547996 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 20:57:08 +0200 Subject: [PATCH 21/40] cmd_vel feedback publisher --- gait_generator/include/gait_generator/gait_generator.hpp | 2 ++ gait_generator/src/gait_generator/gait_generator.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/gait_generator/include/gait_generator/gait_generator.hpp b/gait_generator/include/gait_generator/gait_generator.hpp index 70c74e8..64e4b2e 100644 --- a/gait_generator/include/gait_generator/gait_generator.hpp +++ b/gait_generator/include/gait_generator/gait_generator.hpp @@ -55,6 +55,8 @@ class GaitGenerator { rclcpp::Subscription::SharedPtr cmd_null_pos_subscription_; // null space motion transform (body to // basefootprint) + rclcpp::Publisher::SharedPtr + feedback_cmd_vel_publisher_; rclcpp::TimerBase::SharedPtr timer_; geometry_msgs::msg::Twist cmd_vel_{}; diff --git a/gait_generator/src/gait_generator/gait_generator.cpp b/gait_generator/src/gait_generator/gait_generator.cpp index b97ae0e..41e7299 100644 --- a/gait_generator/src/gait_generator/gait_generator.cpp +++ b/gait_generator/src/gait_generator/gait_generator.cpp @@ -50,6 +50,9 @@ GaitGenerator::GaitGenerator() timer_ = node_->create_wall_timer( std::chrono::milliseconds(static_cast(delta_t_milli)), [this, delta_t_milli]() { timer_callback(delta_t_milli); }); + + feedback_cmd_vel_publisher_ = node_->create_publisher( + "feedback_cmd_vel", 10); } void GaitGenerator::cmd_vel_sub_callback( @@ -170,6 +173,7 @@ void GaitGenerator::update_feet_positions(double delta_t_milli) { void GaitGenerator::timer_callback(double delta_t_milli) { update_phase(delta_t_milli); update_feet_positions(delta_t_milli); + feedback_cmd_vel_publisher_->publish(cmd_vel_); } void GaitGenerator::declare_parameters() { From a8e4a15eb45d6f9ff61a98e24f9ee639fc668873 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 21:12:18 +0200 Subject: [PATCH 22/40] twist to odom --- twist_to_odom/CMakeLists.txt | 76 +++++++++++++ .../include/twist_to_odom/twist_to_odom.hpp | 38 +++++++ twist_to_odom/launch/twist_to_odom.launch.py | 29 +++++ twist_to_odom/package.xml | 26 +++++ .../src/twist_to_odom/twist_to_odom.cpp | 107 ++++++++++++++++++ twist_to_odom/src/twist_to_odom_node.cpp | 8 ++ 6 files changed, 284 insertions(+) create mode 100644 twist_to_odom/CMakeLists.txt create mode 100644 twist_to_odom/include/twist_to_odom/twist_to_odom.hpp create mode 100644 twist_to_odom/launch/twist_to_odom.launch.py create mode 100644 twist_to_odom/package.xml create mode 100644 twist_to_odom/src/twist_to_odom/twist_to_odom.cpp create mode 100644 twist_to_odom/src/twist_to_odom_node.cpp diff --git a/twist_to_odom/CMakeLists.txt b/twist_to_odom/CMakeLists.txt new file mode 100644 index 0000000..26709f5 --- /dev/null +++ b/twist_to_odom/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.12) +project(twist_to_odom) + +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Werror -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +# # Declare battery system driver library +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/${PROJECT_NAME}.cpp + ) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) + + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + ) + + +add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ) + + +############# +## Install ## +############# + +install( + DIRECTORY include/ + DESTINATION include + ) + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} + ) + +##############. +## Testing ## +############## + +if(BUILD_TESTING) +endif() + +ament_package() diff --git a/twist_to_odom/include/twist_to_odom/twist_to_odom.hpp b/twist_to_odom/include/twist_to_odom/twist_to_odom.hpp new file mode 100644 index 0000000..d5e9807 --- /dev/null +++ b/twist_to_odom/include/twist_to_odom/twist_to_odom.hpp @@ -0,0 +1,38 @@ +#ifndef POLYPOD_TWIST_TO_ODOM_HPP_ +#define POLYPOD_TWIST_TO_ODOM_HPP_ + +#include +#include "rclcpp/rclcpp.hpp" // for rclcpp + +// include messages +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" +#include "geometry_msgs/msg/twist_with_covariance.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +namespace polypod::twist_to_odom { + class TwistToOdom { + public: + explicit TwistToOdom(); + void spin() { rclcpp::spin(node_); } + + private: + rclcpp::Node::SharedPtr node_; + + double theta_{}; + + geometry_msgs::msg::Twist cmd_vel_{}; + nav_msgs::msg::Odometry state_{}; + + rclcpp::Subscription::SharedPtr cmd_vel_subscriber_; + rclcpp::Publisher::SharedPtr odometry_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + void declare_parameters(); + }; + +} // namespace polypod::twist_to_odom + +#endif // POLYPOD_TWIST_TO_ODOM_HPP_ \ No newline at end of file diff --git a/twist_to_odom/launch/twist_to_odom.launch.py b/twist_to_odom/launch/twist_to_odom.launch.py new file mode 100644 index 0000000..78c9016 --- /dev/null +++ b/twist_to_odom/launch/twist_to_odom.launch.py @@ -0,0 +1,29 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + name_space_arg = DeclareLaunchArgument( + "name_space", + default_value="", + description="Namespace for the node" + ) + + return LaunchDescription( + [ + name_space_arg, + Node( + package="twist_to_odom", + executable="twist_to_odom_node", + namespace=LaunchConfiguration("name_space"), + output="screen", + remappings=[ + ("feedback_cmd_vel", "feedback_cmd_vel"), + ("odom", "odom"), + ], + ), + ] + ) diff --git a/twist_to_odom/package.xml b/twist_to_odom/package.xml new file mode 100644 index 0000000..3728c53 --- /dev/null +++ b/twist_to_odom/package.xml @@ -0,0 +1,26 @@ + + + + twist_to_odom + 0.0.0 + TODO: Package description + mohammad.safeea + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + nav_msgs + tf2_ros + launch_testing + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/twist_to_odom/src/twist_to_odom/twist_to_odom.cpp b/twist_to_odom/src/twist_to_odom/twist_to_odom.cpp new file mode 100644 index 0000000..8bc34e5 --- /dev/null +++ b/twist_to_odom/src/twist_to_odom/twist_to_odom.cpp @@ -0,0 +1,107 @@ +#include + +// include librarries +#include "twist_to_odom/twist_to_odom.hpp" +#include "rclcpp/rclcpp.hpp" + + +// include messages +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance.hpp" +#include "geometry_msgs/msg/twist_with_covariance.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include + +#define pi 3.141592 + +namespace polypod::twist_to_odom { + + TwistToOdom::TwistToOdom() : node_{rclcpp::Node::make_shared("twist_to_odom_node")} { + RCLCPP_INFO(node_->get_logger(), "Starting cmd_vel to odom node"); + + odometry_publisher_ = node_->create_publisher("odom", 10); + + cmd_vel_subscriber_ = node_->create_subscription( + "feedback_cmd_vel", 1, + [this](const geometry_msgs::msg::Twist& cmd_vel_msg) -> void { + cmd_vel_ = cmd_vel_msg; + }); + + constexpr auto timer_period_millis = 100; + timer_ = node_->create_wall_timer( + std::chrono::milliseconds(timer_period_millis), + [this, timer_period_millis]() -> void { + + auto vx = cmd_vel_.linear.x; + auto vy = cmd_vel_.linear.y; + auto w = cmd_vel_.angular.z; + + constexpr auto millis_to_seconds = 1000.0; + auto vx_dt = vx * (timer_period_millis / millis_to_seconds); + auto vy_dt = vy * (timer_period_millis / millis_to_seconds); + auto w_dt = w * (timer_period_millis / millis_to_seconds); + theta_ += w_dt; + + auto dx = vx_dt * cos(theta_) - vy_dt * sin(theta_); + auto dy = vx_dt * sin(theta_) + vy_dt * cos(theta_); + + state_.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + state_.header.frame_id = "odom"; + state_.child_frame_id = "base_footprint"; + state_.pose.pose.position.x += dx; + state_.pose.pose.position.y += dy; + state_.pose.pose.position.z = 0.0; + state_.pose.pose.orientation.x = 0.0; + state_.pose.pose.orientation.y = 0.0; + state_.pose.pose.orientation.z = sin(theta_/2); + state_.pose.pose.orientation.w = cos(theta_/2); + + // Populate the twist information + state_.twist.twist.linear.x = vx; + state_.twist.twist.linear.y = vy; + state_.twist.twist.angular.z = w; + + state_.pose.covariance = {0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, + 0, 0, 0.01, 0, 0, 0, + 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0.01}; + + state_.twist.covariance = {0.01, 0, 0, 0, 0, 0, + 0, 0.01, 0, 0, 0, 0, + 0, 0, 0.01, 0, 0, 0, + 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0.01}; + + + odometry_publisher_->publish(state_); + + // publish transforms + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = state_.header.stamp; + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "base_footprint"; + + // Set the translation + transformStamped.transform.translation.x = state_.pose.pose.position.x; + transformStamped.transform.translation.y = state_.pose.pose.position.y; + transformStamped.transform.translation.z = state_.pose.pose.position.z; + transformStamped.transform.rotation.x = state_.pose.pose.orientation.x; + transformStamped.transform.rotation.y = state_.pose.pose.orientation.y; + transformStamped.transform.rotation.z = state_.pose.pose.orientation.z; + transformStamped.transform.rotation.w = state_.pose.pose.orientation.w; + // Broadcast the transform + // tf_broadcaster_->publish(transformStamped); + // tf2_ros + static auto br = std::make_shared(node_); + br->sendTransform(transformStamped); + } + ); + } + +} // namespace polypod::twist_to_odom diff --git a/twist_to_odom/src/twist_to_odom_node.cpp b/twist_to_odom/src/twist_to_odom_node.cpp new file mode 100644 index 0000000..9e4caba --- /dev/null +++ b/twist_to_odom/src/twist_to_odom_node.cpp @@ -0,0 +1,8 @@ +#include "twist_to_odom/twist_to_odom.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + polypod::twist_to_odom::TwistToOdom().spin(); + rclcpp::shutdown(); + return 0; +} From 723adc003a3d59dc86e2214eb643f980291b1b24 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 21:12:52 +0200 Subject: [PATCH 23/40] nav2 launcher file --- penta_pod/launch/nav2/nav2_bringup.launch.py | 86 ++++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 penta_pod/launch/nav2/nav2_bringup.launch.py diff --git a/penta_pod/launch/nav2/nav2_bringup.launch.py b/penta_pod/launch/nav2/nav2_bringup.launch.py new file mode 100644 index 0000000..fd153f1 --- /dev/null +++ b/penta_pod/launch/nav2/nav2_bringup.launch.py @@ -0,0 +1,86 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import subprocess + +# This is a launch file used to launch slam nodes on laptop +def generate_launch_description(): + # name space argument + declare_name_space_argument = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + # start odometry + odometry_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("cmd_vel_to_odom"), + "launch/cmd_vel_to_odom.launch.py", + ) + ] + ) + ) + # start slam + use_sim_time = LaunchConfiguration('use_sim_time') + scan_topic = [LaunchConfiguration('name_space'), '/scan'] + map_frame = [LaunchConfiguration('name_space'), 'map'] + base_frame = [LaunchConfiguration('name_space'), 'base_footprint'] + odom_frame = [LaunchConfiguration('name_space'), 'odom'] + slam_params_file = LaunchConfiguration('slam_params_file') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("penta_pod"), #"slam_toolbox"), + 'config', 'mapper_params_online_async.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + start_async_slam_toolbox_node = Node( + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time}, + {'odom_frame': odom_frame}, + {'map_frame': map_frame}, + {'base_frame': base_frame}, + {'scan_topic': scan_topic}, + {'use_map_saver': True}, + {'mode': 'mapping'} + ], + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("nav2_bringup"), + "launch/navigation_launch.py", + ) + ] + ) + ) + + ld = LaunchDescription() + ld.add_action(declare_name_space_argument) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(odometry_launch) + ld.add_action(start_async_slam_toolbox_node) + ld.add_action(nav2_launch) + + return ld From 798828dfda0bc850258872259ce67cf0798bc501 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 21:13:52 +0200 Subject: [PATCH 24/40] configuration for mapping launch --- .../config/mapper_params_online_async.yaml | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 penta_pod/config/mapper_params_online_async.yaml diff --git a/penta_pod/config/mapper_params_online_async.yaml b/penta_pod/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..f69adf4 --- /dev/null +++ b/penta_pod/config/mapper_params_online_async.yaml @@ -0,0 +1,75 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + # Following parameters are moved to the launch file + # odom_frame: odom + # map_frame: map + # base_frame: base_footprint + # scan_topic: /scan + # use_map_saver: true + # mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 10.0 # rplidar A1 is 12 meters + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true From ef02c464a8d86dbcb4a790110b256ee1016d1721 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 21:15:06 +0200 Subject: [PATCH 25/40] rename cmd_vel_to_odom into twist_to_odom --- cmd_vel_to_odom/CMakeLists.txt | 76 ------------- .../cmd_vel_to_odom/cmd_vel_to_odom.hpp | 38 ------- .../launch/cmd_vel_to_odom.launch.py | 29 ----- cmd_vel_to_odom/package.xml | 26 ----- .../src/cmd_vel_to_odom/cmd_vel_to_odom.cpp | 107 ------------------ cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp | 8 -- 6 files changed, 284 deletions(-) delete mode 100644 cmd_vel_to_odom/CMakeLists.txt delete mode 100644 cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp delete mode 100644 cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py delete mode 100644 cmd_vel_to_odom/package.xml delete mode 100644 cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp delete mode 100644 cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp diff --git a/cmd_vel_to_odom/CMakeLists.txt b/cmd_vel_to_odom/CMakeLists.txt deleted file mode 100644 index 946ae22..0000000 --- a/cmd_vel_to_odom/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -cmake_minimum_required(VERSION 3.12) -project(cmd_vel_to_odom) - -# Default to C++20 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Werror -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) - -# # Declare battery system driver library -add_library(${PROJECT_NAME} - src/${PROJECT_NAME}/${PROJECT_NAME}.cpp - ) - -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $) - - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - std_msgs - geometry_msgs - nav_msgs - tf2 - tf2_ros - ) - - -add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} - ) - - -############# -## Install ## -############# - -install( - DIRECTORY include/ - DESTINATION include - ) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} - ) - -##############. -## Testing ## -############## - -if(BUILD_TESTING) -endif() - -ament_package() diff --git a/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp b/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp deleted file mode 100644 index 787c263..0000000 --- a/cmd_vel_to_odom/include/cmd_vel_to_odom/cmd_vel_to_odom.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef POLYPOD_CMD_VEL_TO_ODOM_HPP_ -#define POLYPOD_CMD_VEL_TO_ODOM_HPP_ - -#include -#include "rclcpp/rclcpp.hpp" // for rclcpp - -// include messages -#include "geometry_msgs/msg/twist.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/pose_with_covariance.hpp" -#include "geometry_msgs/msg/twist_with_covariance.hpp" - -#include "tf2_ros/transform_broadcaster.h" - -namespace polypod::cmd_vel_to_odom { - class CmdVelToOdom { - public: - explicit CmdVelToOdom(); - void spin() { rclcpp::spin(node_); } - - private: - rclcpp::Node::SharedPtr node_; - - double theta_{}; - - geometry_msgs::msg::Twist cmd_vel_{}; - nav_msgs::msg::Odometry state_{}; - - rclcpp::Subscription::SharedPtr cmd_vel_subscriber_; - rclcpp::Publisher::SharedPtr odometry_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - - void declare_parameters(); - }; - -} // namespace polypod::cmd_vel_to_odom - -#endif // POLYPOD_CMD_VEL_TO_ODOM_HPP_ \ No newline at end of file diff --git a/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py b/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py deleted file mode 100644 index 8ace09e..0000000 --- a/cmd_vel_to_odom/launch/cmd_vel_to_odom.launch.py +++ /dev/null @@ -1,29 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration - -def generate_launch_description(): - name_space_arg = DeclareLaunchArgument( - "name_space", - default_value="", - description="Namespace for the node" - ) - - return LaunchDescription( - [ - name_space_arg, - Node( - package="cmd_vel_to_odom", - executable="cmd_vel_to_odom_node", - namespace=LaunchConfiguration("name_space"), - output="screen", - remappings=[ - ("feedback_cmd_vel", "feedback_cmd_vel"), - ("odom", "odom"), - ], - ), - ] - ) diff --git a/cmd_vel_to_odom/package.xml b/cmd_vel_to_odom/package.xml deleted file mode 100644 index 254547e..0000000 --- a/cmd_vel_to_odom/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - cmd_vel_to_odom - 0.0.0 - TODO: Package description - mohammad.safeea - TODO: License declaration - - ament_cmake - - rclcpp - std_msgs - geometry_msgs - nav_msgs - tf2_ros - launch_testing - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - diff --git a/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp b/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp deleted file mode 100644 index f1c4514..0000000 --- a/cmd_vel_to_odom/src/cmd_vel_to_odom/cmd_vel_to_odom.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include - -// include librarries -#include "cmd_vel_to_odom/cmd_vel_to_odom.hpp" -#include "rclcpp/rclcpp.hpp" - - -// include messages -#include "geometry_msgs/msg/twist.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/pose_with_covariance.hpp" -#include "geometry_msgs/msg/twist_with_covariance.hpp" - -#include "tf2_ros/transform_broadcaster.h" - -#include - -#define pi 3.141592 - -namespace polypod::cmd_vel_to_odom { - - CmdVelToOdom::CmdVelToOdom() : node_{rclcpp::Node::make_shared("cmd_vel_to_odom_node")} { - RCLCPP_INFO(node_->get_logger(), "Starting cmd_vel to odom node"); - - odometry_publisher_ = node_->create_publisher("odom", 10); - - cmd_vel_subscriber_ = node_->create_subscription( - "feedback_cmd_vel", 1, - [this](const geometry_msgs::msg::Twist& cmd_vel_msg) -> void { - cmd_vel_ = cmd_vel_msg; - }); - - constexpr auto timer_period_millis = 100; - timer_ = node_->create_wall_timer( - std::chrono::milliseconds(timer_period_millis), - [this, timer_period_millis]() -> void { - - auto vx = cmd_vel_.linear.x; - auto vy = cmd_vel_.linear.y; - auto w = cmd_vel_.angular.z; - - constexpr auto millis_to_seconds = 1000.0; - auto vx_dt = vx * (timer_period_millis / millis_to_seconds); - auto vy_dt = vy * (timer_period_millis / millis_to_seconds); - auto w_dt = w * (timer_period_millis / millis_to_seconds); - theta_ += w_dt; - - auto dx = vx_dt * cos(theta_) - vy_dt * sin(theta_); - auto dy = vx_dt * sin(theta_) + vy_dt * cos(theta_); - - state_.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); - state_.header.frame_id = "odom"; - state_.child_frame_id = "base_footprint"; - state_.pose.pose.position.x += dx; - state_.pose.pose.position.y += dy; - state_.pose.pose.position.z = 0.0; - state_.pose.pose.orientation.x = 0.0; - state_.pose.pose.orientation.y = 0.0; - state_.pose.pose.orientation.z = sin(theta_/2); - state_.pose.pose.orientation.w = cos(theta_/2); - - // Populate the twist information - state_.twist.twist.linear.x = vx; - state_.twist.twist.linear.y = vy; - state_.twist.twist.angular.z = w; - - state_.pose.covariance = {0.01, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, - 0, 0, 0.01, 0, 0, 0, - 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0.01}; - - state_.twist.covariance = {0.01, 0, 0, 0, 0, 0, - 0, 0.01, 0, 0, 0, 0, - 0, 0, 0.01, 0, 0, 0, - 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0.01}; - - - odometry_publisher_->publish(state_); - - // publish transforms - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = state_.header.stamp; - transformStamped.header.frame_id = "odom"; - transformStamped.child_frame_id = "base_footprint"; - - // Set the translation - transformStamped.transform.translation.x = state_.pose.pose.position.x; - transformStamped.transform.translation.y = state_.pose.pose.position.y; - transformStamped.transform.translation.z = state_.pose.pose.position.z; - transformStamped.transform.rotation.x = state_.pose.pose.orientation.x; - transformStamped.transform.rotation.y = state_.pose.pose.orientation.y; - transformStamped.transform.rotation.z = state_.pose.pose.orientation.z; - transformStamped.transform.rotation.w = state_.pose.pose.orientation.w; - // Broadcast the transform - // tf_broadcaster_->publish(transformStamped); - // tf2_ros - static auto br = std::make_shared(node_); - br->sendTransform(transformStamped); - } - ); - } - -} // namespace polypod::cmd_vel_to_odom diff --git a/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp b/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp deleted file mode 100644 index 1c12548..0000000 --- a/cmd_vel_to_odom/src/cmd_vel_to_odom_node.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "cmd_vel_to_odom/cmd_vel_to_odom.hpp" - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - polypod::cmd_vel_to_odom::CmdVelToOdom().spin(); - rclcpp::shutdown(); - return 0; -} From 3a4f29ce0d8b76c170f59e2bc5fae31c146f08c6 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 27 Jul 2025 22:40:48 +0200 Subject: [PATCH 26/40] add missing packages --- .devcontainer/Dockerfile | 52 +++++++++++++++++++++++++++++-- .devcontainer/docker-compose.yaml | 3 ++ 2 files changed, 53 insertions(+), 2 deletions(-) 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..024f03b 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -10,6 +10,9 @@ services: environment: - ROS_DOMAIN_ID=91 network_mode: "host" + devices: + - "/dev/i2c-1:/dev/i2c-1" + privileged: true command: ["/bin/bash", "-c", "/root/ros_ws/src/.devcontainer/entrypoint.sh"] scorpion-ros2-test: From b68a19e05aec9fcbbe6ef3e2497eeed28a0c8153 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 10:13:30 +0200 Subject: [PATCH 27/40] add map and laser scan --- penta_description/config/penta.rviz | 163 ++++++++++++++++------------ 1 file changed, 93 insertions(+), 70 deletions(-) diff --git a/penta_description/config/penta.rviz b/penta_description/config/penta.rviz index e02ee8c..03511b1 100644 --- a/penta_description/config/penta.rviz +++ b/penta_description/config/penta.rviz @@ -1,11 +1,11 @@ Panels: - Class: rviz_common/Displays - Help Height: 70 + Help Height: 71 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 296 + Tree Height: 295 - Class: rviz_common/Views Expanded: - /Current View1 @@ -59,6 +59,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + head_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb0_lower_arm_link: Alpha: 1 Show Axes: false @@ -139,6 +144,26 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb4_lower_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + limb4_shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + limb4_shoulder_motor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + limb4_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -148,75 +173,73 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz_default_plugins/TF - Enabled: true + Enabled: false Frame Timeout: 15 Frames: All Enabled: true - base_footprint: - Value: true - base_link: - Value: true - limb0_lower_arm_link: - Value: true - limb0_shoulder_link: - Value: true - limb0_shoulder_motor: - Value: true - limb0_upper_arm_link: - Value: true - limb1_lower_arm_link: - Value: true - limb1_shoulder_link: - Value: true - limb1_shoulder_motor: - Value: true - limb1_upper_arm_link: - Value: true - limb2_lower_arm_link: - Value: true - limb2_shoulder_link: - Value: true - limb2_shoulder_motor: - Value: true - limb2_upper_arm_link: - Value: true - limb3_lower_arm_link: - Value: true - limb3_shoulder_link: - Value: true - limb3_shoulder_motor: - Value: true - limb3_upper_arm_link: - Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - base_footprint: - base_link: - limb0_shoulder_motor: - limb0_shoulder_link: - limb0_upper_arm_link: - limb0_lower_arm_link: - {} - limb1_shoulder_motor: - limb1_shoulder_link: - limb1_upper_arm_link: - limb1_lower_arm_link: - {} - limb2_shoulder_motor: - limb2_shoulder_link: - limb2_upper_arm_link: - limb2_lower_arm_link: - {} - limb3_shoulder_motor: - limb3_shoulder_link: - limb3_upper_arm_link: - limb3_lower_arm_link: - {} + {} Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 47 + Min Color: 0; 0; 0 + Min Intensity: 47 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false Value: true Enabled: true Global Options: @@ -233,25 +256,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.7000000476837158 + Distance: 3.2102811336517334 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.04504045099020004 + Y: -0.22101935744285583 + Z: -0.011066662147641182 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6899999380111694 + Pitch: 0.684999942779541 Target Frame: Value: Orbit (rviz) - Yaw: 5.414999961853027 + Yaw: 2.5849883556365967 Saved: ~ Window Geometry: Displays: @@ -259,9 +282,9 @@ Window Geometry: Height: 800 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001cb000002c6fc0200000002fb000000100044006900730070006c006100790073010000003d000001ab000000c900fffffffb0000000a0056006900650077007301000001ee00000115000000a400ffffff000002df000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001cb000002c6fc0200000002fb000000100044006900730070006c006100790073010000003d000001ab000000c900fffffffb0000000a0056006900650077007301000001ee00000115000000a400ffffff00000397000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1200 - X: 413 + Width: 1384 + X: 229 Y: 99 From 585fd025bc70242214f8326786ec3302253ad738 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 10:14:21 +0200 Subject: [PATCH 28/40] nav2 nodes --- penta_pod/launch/nav2/nav2_bringup.launch.py | 67 ++++++++++--------- .../launch/nav2/nav2_util_nodes.launch.py | 56 ++++++++++++++++ 2 files changed, 91 insertions(+), 32 deletions(-) create mode 100644 penta_pod/launch/nav2/nav2_util_nodes.launch.py diff --git a/penta_pod/launch/nav2/nav2_bringup.launch.py b/penta_pod/launch/nav2/nav2_bringup.launch.py index fd153f1..c55655e 100644 --- a/penta_pod/launch/nav2/nav2_bringup.launch.py +++ b/penta_pod/launch/nav2/nav2_bringup.launch.py @@ -9,7 +9,6 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -import subprocess # This is a launch file used to launch slam nodes on laptop def generate_launch_description(): @@ -19,51 +18,55 @@ def generate_launch_description(): default_value="", description="Robot name space", ) - # start odometry - odometry_launch = IncludeLaunchDescription( + # start utils nodes + utils_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ os.path.join( - get_package_share_directory("cmd_vel_to_odom"), - "launch/cmd_vel_to_odom.launch.py", + get_package_share_directory("penta_pod"), + "launch/nav2/nav2_util_nodes.launch.py", ) ] ) ) # start slam - use_sim_time = LaunchConfiguration('use_sim_time') - scan_topic = [LaunchConfiguration('name_space'), '/scan'] - map_frame = [LaunchConfiguration('name_space'), 'map'] - base_frame = [LaunchConfiguration('name_space'), 'base_footprint'] - odom_frame = [LaunchConfiguration('name_space'), 'odom'] - slam_params_file = LaunchConfiguration('slam_params_file') + use_sim_time = LaunchConfiguration("use_sim_time") + scan_topic = [LaunchConfiguration("name_space"), "/scan"] + map_frame = [LaunchConfiguration("name_space"), "map"] + base_frame = [LaunchConfiguration("name_space"), "base_footprint"] + odom_frame = [LaunchConfiguration("name_space"), "odom"] + slam_params_file = LaunchConfiguration("slam_params_file") declare_use_sim_time_argument = DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation/Gazebo clock') + "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" + ) declare_slam_params_file_cmd = DeclareLaunchArgument( - 'slam_params_file', - default_value=os.path.join(get_package_share_directory("penta_pod"), #"slam_toolbox"), - 'config', 'mapper_params_online_async.yaml'), - description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + "slam_params_file", + default_value=os.path.join( + get_package_share_directory("penta_pod"), # "slam_toolbox"), + "config", + "mapper_params_online_async.yaml", + ), + description="Full path to the ROS2 parameters file to use for the slam_toolbox node", + ) start_async_slam_toolbox_node = Node( parameters=[ - slam_params_file, - {'use_sim_time': use_sim_time}, - {'odom_frame': odom_frame}, - {'map_frame': map_frame}, - {'base_frame': base_frame}, - {'scan_topic': scan_topic}, - {'use_map_saver': True}, - {'mode': 'mapping'} + slam_params_file, + {"use_sim_time": use_sim_time}, + {"odom_frame": odom_frame}, + {"map_frame": map_frame}, + {"base_frame": base_frame}, + {"scan_topic": scan_topic}, + {"use_map_saver": True}, + {"mode": "mapping"}, ], - package='slam_toolbox', - executable='async_slam_toolbox_node', - name='slam_toolbox', - output='screen') - + package="slam_toolbox", + executable="async_slam_toolbox_node", + name="slam_toolbox", + output="screen", + ) + nav2_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -79,7 +82,7 @@ def generate_launch_description(): ld.add_action(declare_name_space_argument) ld.add_action(declare_use_sim_time_argument) ld.add_action(declare_slam_params_file_cmd) - ld.add_action(odometry_launch) + ld.add_action(utils_launch) ld.add_action(start_async_slam_toolbox_node) ld.add_action(nav2_launch) diff --git a/penta_pod/launch/nav2/nav2_util_nodes.launch.py b/penta_pod/launch/nav2/nav2_util_nodes.launch.py new file mode 100644 index 0000000..2985682 --- /dev/null +++ b/penta_pod/launch/nav2/nav2_util_nodes.launch.py @@ -0,0 +1,56 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # name space argument + declare_name_space_argument = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + + return LaunchDescription([ + declare_name_space_argument, + # Declare launch arguments for frame names + DeclareLaunchArgument('parent_frame', default_value='base_link'), + DeclareLaunchArgument('child_frame', default_value='laser'), + + # Declare launch arguments for transform (optional) + DeclareLaunchArgument('x', default_value='0.0'), + DeclareLaunchArgument('y', default_value='0.0'), + DeclareLaunchArgument('z', default_value='0.0'), + DeclareLaunchArgument('qx', default_value='0.0'), + DeclareLaunchArgument('qy', default_value='0.0'), + DeclareLaunchArgument('qz', default_value='0.0'), + DeclareLaunchArgument('qw', default_value='1.0'), + + # Node that runs static_transform_publisher + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_pub', + arguments=[ + LaunchConfiguration('x'), + LaunchConfiguration('y'), + LaunchConfiguration('z'), + LaunchConfiguration('qx'), + LaunchConfiguration('qy'), + LaunchConfiguration('qz'), + LaunchConfiguration('qw'), + [LaunchConfiguration('name_space'), LaunchConfiguration('parent_frame')], + [LaunchConfiguration('name_space'), LaunchConfiguration('child_frame')], + ] + ), + Node( + package="twist_to_odom", + executable="twist_to_odom_node", + namespace=LaunchConfiguration("name_space"), + output="screen", + remappings=[ + ("feedback_cmd_vel", "feedback_cmd_vel"), + ("odom", "odom"), + ], + ), + ]) From a5c7170ff34f37c954d02e272d91a71447e6177a Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 10:44:07 +0200 Subject: [PATCH 29/40] adding notes on how to enable nav2 --- penta_pod/launch/nav2/README.md | 64 +++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 penta_pod/launch/nav2/README.md diff --git a/penta_pod/launch/nav2/README.md b/penta_pod/launch/nav2/README.md new file mode 100644 index 0000000..c9e8945 --- /dev/null +++ b/penta_pod/launch/nav2/README.md @@ -0,0 +1,64 @@ +# About + +This is nav2 launcher file for the penta_pod robot, + +To work, you ineed to install rplidar on the robot and connect it to USB port of the raspberry pi + + +# What to launch on the robot + +Turn on the robot hardware using: + +``` +ros2 launch penta_pod realhardware_bringup.launch.py +``` + + +Turn on rplidar on the robot: + +``` +ros2 launch rplidar_ros rplidar.launch.py +``` + +To install rplidar on the robot raspberry pi: + +``` +cd ~/your_workspace/src + +git clone https://github.com/babakhani/rplidar_ros2.git + +cd ~/your_workspace + +colcon build +``` + +# What to launch on an external PC + + +On an external computer running NAV2 run the following: + +``` +ros2 launch penta_pod nav2_bringup.launch.py +``` + + +# Notes + +Make sure of the following + +- your PC and the robot are connected on same wifi + +- you are using cyclone dds on the robot and on the external pc + +- you have the same `ROS_DOAMIN_ID` on both the PC and the raspberry-pi, to do so do the following both on raspberry-pi and on your PC + +``` +nano ~/.bashrc +``` + +then add the following at the end of the file + +``` +export ROS_DOAMIN_ID=38 +``` + From 58894f91ce99a46f3f823bc8928242391ae4abe3 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 11:15:40 +0200 Subject: [PATCH 30/40] fix rviz file --- penta_description/config/penta.rviz | 126 +++++++++++++++++++++------- 1 file changed, 95 insertions(+), 31 deletions(-) diff --git a/penta_description/config/penta.rviz b/penta_description/config/penta.rviz index 1edb19f..948bc53 100644 --- a/penta_description/config/penta.rviz +++ b/penta_description/config/penta.rviz @@ -1,12 +1,13 @@ Panels: - Class: rviz_common/Displays - Help Height: 71 + Help Height: 70 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /Map1 Splitter Ratio: 0.5 - Tree Height: 295 + Tree Height: 617 - Class: rviz_common/Views Expanded: - /Current View1 @@ -65,7 +66,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - head_link: + limb0_foot: Alpha: 1 Show Axes: false Show Trail: false @@ -90,6 +91,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb1_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb1_lower_arm_link: Alpha: 1 Show Axes: false @@ -110,6 +116,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb2_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb2_lower_arm_link: Alpha: 1 Show Axes: false @@ -130,62 +141,62 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - limb3_lower_arm_link: + limb3_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb3_shoulder_link: + limb3_lower_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb3_shoulder_motor: + limb3_shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb3_upper_arm_link: + limb3_shoulder_motor: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_lower_arm_link: + limb3_upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_shoulder_link: + limb4_foot: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_shoulder_motor: + limb4_lower_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_upper_arm_link: + limb4_shoulder_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_lower_arm_link: + limb4_shoulder_motor: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_shoulder_link: + limb4_upper_arm_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_shoulder_motor: + lower_plate_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - limb4_upper_arm_link: + upper_plate_link: Alpha: 1 Show Axes: false Show Trail: false @@ -199,7 +210,6 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz_default_plugins/TF - Enabled: false Enabled: false Frame Timeout: 15 Frames: @@ -211,9 +221,63 @@ Visualization Manager: Show Names: false Tree: {} - {} Update Interval: 0 Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 47 + Min Color: 0; 0; 0 + Min Intensity: 47 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -229,35 +293,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 0.7511907815933228 + Distance: 2.481476068496704 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.01839996874332428 - Y: -0.016720440238714218 - Z: 0.056901950389146805 + X: -0.1272813081741333 + Y: -0.07276946306228638 + Z: 0.14693914353847504 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.49500012397766113 + Pitch: 0.6599998474121094 Target Frame: Value: Orbit (rviz) - Yaw: 2.1449995040893555 + Yaw: 2.3550095558166504 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 800 - Hide Left Dock: true + collapsed: false + Height: 1136 + Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001cb000002c6fc0200000002fb000000100044006900730070006c006100790073000000003d000001ab000000c900fffffffb0000000a0056006900650077007300000001ee00000115000000a400ffffff000004b0000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001cb00000416fc0200000002fb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000000a00560069006500770073010000032f00000124000000a400ffffff000005690000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1200 - X: 413 - Y: 99 + collapsed: false + Width: 1850 + X: 70 + Y: 27 From bc5bd2d17a44532d81571f36b593279deb110377 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 14:02:33 +0200 Subject: [PATCH 31/40] readme on how to enable navigation on the robot using ROS2 --- penta_pod/launch/nav2/README.md | 42 ++++++++++++++++++++------------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/penta_pod/launch/nav2/README.md b/penta_pod/launch/nav2/README.md index c9e8945..ad6109c 100644 --- a/penta_pod/launch/nav2/README.md +++ b/penta_pod/launch/nav2/README.md @@ -1,54 +1,62 @@ # About -This is nav2 launcher file for the penta_pod robot, +Setting up autonomous navigation for the penta_pod robot, -To work, you ineed to install rplidar on the robot and connect it to USB port of the raspberry pi +Hardware requirement: -# What to launch on the robot +- pentapod robot connected to wifi -Turn on the robot hardware using: +- external pc connected to same netwrok as robot + +- rplidar A1, mounted on the robot and connected to raspberry pi of the pentapod robot + + +Software requirement, you have to perforn sinple installation steps both on the robot and the PC as in the following: -``` -ros2 launch penta_pod realhardware_bringup.launch.py -``` +# Software installion on the robot -Turn on rplidar on the robot: +To install navigation stack NAV2 on PC, from inside the folder [scripts](./polypod/penta_pod/launch/nav2/scripts/) run the installation script: ``` -ros2 launch rplidar_ros rplidar.launch.py +./install_rplidar_on_robot.sh ``` -To install rplidar on the robot raspberry pi: +# Software installion on PC +To install navigation stack NAV2 on PC, from inside the folder [scripts](./polypod/penta_pod/launch/nav2/scripts/) run the installation script: + +``` +./install_nav2_on_pc.sh ``` -cd ~/your_workspace/src -git clone https://github.com/babakhani/rplidar_ros2.git +# What to launch on the robot -cd ~/your_workspace +Turn on the robot hardware using: -colcon build ``` +ros2 launch penta_pod realhardware_bringup.launch.py +``` + # What to launch on an external PC -On an external computer running NAV2 run the following: +To run the navigation and visualize with Rviz, On an external computer running NAV2 run the following: ``` ros2 launch penta_pod nav2_bringup.launch.py ``` -# Notes +# Extra considerations Make sure of the following - your PC and the robot are connected on same wifi -- you are using cyclone dds on the robot and on the external pc +- you are using cyclone dds both on the robot and on the external pc - you have the same `ROS_DOAMIN_ID` on both the PC and the raspberry-pi, to do so do the following both on raspberry-pi and on your PC From 24e191b94235877bff348dd509ec2e2f393caa5d Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 14:03:00 +0200 Subject: [PATCH 32/40] add rviz to show the robot and the map --- penta_pod/launch/nav2/nav2_bringup.launch.py | 21 ++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/penta_pod/launch/nav2/nav2_bringup.launch.py b/penta_pod/launch/nav2/nav2_bringup.launch.py index c55655e..072259e 100644 --- a/penta_pod/launch/nav2/nav2_bringup.launch.py +++ b/penta_pod/launch/nav2/nav2_bringup.launch.py @@ -3,6 +3,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory @@ -18,6 +19,12 @@ def generate_launch_description(): default_value="", description="Robot name space", ) + # use gazebo simulation argument + use_gazebo_simulation_arg = DeclareLaunchArgument( + "use_gazebo_simulation", + default_value="False", + description="Change to true to use Gazebo", + ) # start utils nodes utils_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -78,12 +85,26 @@ def generate_launch_description(): ) ) + # 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'), + 'use_gazebo_simulation': LaunchConfiguration('use_gazebo_simulation'), + 'joint_states_remappings': 'joint_setpoints', + }.items() + ) + ld = LaunchDescription() ld.add_action(declare_name_space_argument) + ld.add_action(use_gazebo_simulation_arg) ld.add_action(declare_use_sim_time_argument) ld.add_action(declare_slam_params_file_cmd) ld.add_action(utils_launch) ld.add_action(start_async_slam_toolbox_node) ld.add_action(nav2_launch) + ld.add_action(rviz_penta_pod_launch) return ld From 4588c10b0b2184dbf91bd5532f2de8a3ded3b80d Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 14:03:46 +0200 Subject: [PATCH 33/40] nav2 instllation scripts on the robot and on the PC --- .../launch/nav2/scripts/install_nav2_on_pc.sh | 29 +++++++++++++++++++ .../nav2/scripts/install_rplidar_on_robot.sh | 20 +++++++++++++ .../launch/nav2/scripts/penta_nav_repos.yaml | 5 ++++ 3 files changed, 54 insertions(+) create mode 100755 penta_pod/launch/nav2/scripts/install_nav2_on_pc.sh create mode 100755 penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh create mode 100644 penta_pod/launch/nav2/scripts/penta_nav_repos.yaml diff --git a/penta_pod/launch/nav2/scripts/install_nav2_on_pc.sh b/penta_pod/launch/nav2/scripts/install_nav2_on_pc.sh new file mode 100755 index 0000000..8092c16 --- /dev/null +++ b/penta_pod/launch/nav2/scripts/install_nav2_on_pc.sh @@ -0,0 +1,29 @@ +#! /bin/bash + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd "$SCRIPT_DIR" + +# Extract workspace root directory, by splitting on first src directory +WORKSPACE_ROOT=$(awk -F'src' '{print $1}' <<< "$SCRIPT_DIR") + +ROS_DISTRO=humble + +# Install Nav2 on PC +# This script is used to install the necessary packages for Nav2 on a PC. +sudo apt update +sudo apt install -y \ + ros-$ROS_DISTRO-navigation2 \ + ros-$ROS_DISTRO-nav2-bringup \ + ros-$ROS_DISTRO-nav2-common \ + ros-$ROS_DISTRO-nav2-costmap-2d \ + +# install vcs tools +sudo apt install -y python3-vcstool +sudo apt install -y python3-colcon-common-extensions + +# clone nav2 repos +vcs import ${WORKSPACE_ROOT}/src < ./penta_nav_repos.yaml + +# Build the workspace +cd $WORKSPACE_ROOT +colcon build \ No newline at end of file diff --git a/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh b/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh new file mode 100755 index 0000000..82aea99 --- /dev/null +++ b/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh @@ -0,0 +1,20 @@ +#! /bin/bash + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd "$SCRIPT_DIR" + +# Extract workspace root directory, by splitting on first src directory +WORKSPACE_ROOT=$(awk -F'src' '{print $1}' <<< "$SCRIPT_DIR") + +ROS_DISTRO=humble + +# install vcs tools +sudo apt install -y python3-vcstool +sudo apt install -y python3-colcon-common-extensions + +# clone nav2 repos +vcs import ${WORKSPACE_ROOT}/src < ./penta_nav_repos.yaml + +# Build the workspace +cd $WORKSPACE_ROOT +colcon build \ No newline at end of file diff --git a/penta_pod/launch/nav2/scripts/penta_nav_repos.yaml b/penta_pod/launch/nav2/scripts/penta_nav_repos.yaml new file mode 100644 index 0000000..0a94298 --- /dev/null +++ b/penta_pod/launch/nav2/scripts/penta_nav_repos.yaml @@ -0,0 +1,5 @@ +repositories: + rplidar_ros2: + type: git + url: https://github.com/babakhani/rplidar_ros2.git + version: ros2 From 9956fe17a97a6183ab676656415a6919a6f7d243 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 14:04:09 +0200 Subject: [PATCH 34/40] add rplidar launch --- .../launch/nav2/penta_pod_rplidar.launch.py | 44 +++++++++++++++++++ .../launch/realhardware_bringup.launch.py | 15 ++++++- 2 files changed, 57 insertions(+), 2 deletions(-) create mode 100644 penta_pod/launch/nav2/penta_pod_rplidar.launch.py diff --git a/penta_pod/launch/nav2/penta_pod_rplidar.launch.py b/penta_pod/launch/nav2/penta_pod_rplidar.launch.py new file mode 100644 index 0000000..f8e4e3a --- /dev/null +++ b/penta_pod/launch/nav2/penta_pod_rplidar.launch.py @@ -0,0 +1,44 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory, PackageNotFoundError + +def generate_launch_description(): + """ + Check if rplidar_ros package is installed. + """ + try: + get_package_share_directory('rplidar_ros') + except PackageNotFoundError as e: + # Use a LogInfo action so it's visible in ros2 launch output + return LaunchDescription([ + LogInfo(msg="RPLIDAR ROS package not found. Skipping rplidar node.") + ]) + + # Declare namespace argument + declare_name_space_argument = DeclareLaunchArgument( + "name_space", + default_value="", + description="Robot name space", + ) + + rplidar_node = Node( + name='rplidar_composition', + package='rplidar_ros', + executable='rplidar_composition', + output='screen', + parameters=[{ + 'serial_port': '/dev/ttyUSB0', + 'serial_baudrate': 115200, # A1 / A2 + # 'serial_baudrate': 256000, # A3 + 'frame_id': [LaunchConfiguration('name_space'), 'laser'], + 'inverted': False, + 'angle_compensate': True, + }], + ) + + return LaunchDescription([ + declare_name_space_argument, + rplidar_node, + ]) diff --git a/penta_pod/launch/realhardware_bringup.launch.py b/penta_pod/launch/realhardware_bringup.launch.py index 05e58b3..450932d 100644 --- a/penta_pod/launch/realhardware_bringup.launch.py +++ b/penta_pod/launch/realhardware_bringup.launch.py @@ -15,10 +15,10 @@ def generate_launch_description(): description="Robot name space", ) - # Declare the 'mode' argument with a default value of 'real' + # Declare the 'mode' argument with a default value of 'real' otherwise 'virtual' mode_arg = DeclareLaunchArgument( 'mode', - default_value='real', + default_value='real', # 'virtual' description='Mode to run the actuators (real or sim)' ) @@ -43,9 +43,20 @@ def generate_launch_description(): }.items() # Pass the argyments ) + # Include the penta_rplidar launch file + penta_rplidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(FindPackageShare('penta_pod').find('penta_pod'), 'launch', 'nav2', 'penta_pod_rplidar.launch.py') + ), + launch_arguments={ + 'name_space': LaunchConfiguration('name_space'), + }.items() + ) + return LaunchDescription([ name_space_arg, mode_arg, penta_core_launch, penta_i2c_actuators_launch, + penta_rplidar_launch, ]) From fd9756b02e13834516258454e0f1ea970e860e6d Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 20:22:25 +0200 Subject: [PATCH 35/40] only relevant packages has to be colcon build --- penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh b/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh index 82aea99..dd4be2a 100755 --- a/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh +++ b/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh @@ -17,4 +17,5 @@ vcs import ${WORKSPACE_ROOT}/src < ./penta_nav_repos.yaml # Build the workspace cd $WORKSPACE_ROOT -colcon build \ No newline at end of file +colcon build --packages-select rplidar_ros +colcon build --packages-up-to penta_pod --parallel-workers 1 \ No newline at end of file From bc43e27ba612b5e7fdce5404d90b126008cc171e Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 20:24:33 +0200 Subject: [PATCH 36/40] fix path --- penta_pod/launch/nav2/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/penta_pod/launch/nav2/README.md b/penta_pod/launch/nav2/README.md index ad6109c..5d109bf 100644 --- a/penta_pod/launch/nav2/README.md +++ b/penta_pod/launch/nav2/README.md @@ -17,7 +17,7 @@ Software requirement, you have to perforn sinple installation steps both on the # Software installion on the robot -To install navigation stack NAV2 on PC, from inside the folder [scripts](./polypod/penta_pod/launch/nav2/scripts/) run the installation script: +To install navigation stack NAV2 on PC, from inside the folder [scripts](./scripts/) run the installation script: ``` ./install_rplidar_on_robot.sh @@ -25,7 +25,7 @@ To install navigation stack NAV2 on PC, from inside the folder [scripts](./polyp # Software installion on PC -To install navigation stack NAV2 on PC, from inside the folder [scripts](./polypod/penta_pod/launch/nav2/scripts/) run the installation script: +To install navigation stack NAV2 on PC, from inside the folder [scripts](./scripts/) run the installation script: ``` ./install_nav2_on_pc.sh From 25601c254e41a589a7e72f6c7aac695d6f707c68 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sat, 2 Aug 2025 20:51:11 +0200 Subject: [PATCH 37/40] fix doco --- penta_pod/launch/nav2/README.md | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/penta_pod/launch/nav2/README.md b/penta_pod/launch/nav2/README.md index 5d109bf..8fe391f 100644 --- a/penta_pod/launch/nav2/README.md +++ b/penta_pod/launch/nav2/README.md @@ -2,28 +2,25 @@ Setting up autonomous navigation for the penta_pod robot, - Hardware requirement: - pentapod robot connected to wifi -- external pc connected to same netwrok as robot +- external pc connected to same network as robot - rplidar A1, mounted on the robot and connected to raspberry pi of the pentapod robot +Software requirement, you have to perform simple installation steps both on the robot and the PC as in the following: -Software requirement, you have to perforn sinple installation steps both on the robot and the PC as in the following: - +# Software installation on the robot -# Software installion on the robot - -To install navigation stack NAV2 on PC, from inside the folder [scripts](./scripts/) run the installation script: +To install software on the robot raspberry-pi, from inside the folder [scripts](./scripts/) run the installation script: ``` ./install_rplidar_on_robot.sh ``` -# Software installion on PC +# Software installation on PC To install navigation stack NAV2 on PC, from inside the folder [scripts](./scripts/) run the installation script: @@ -58,7 +55,7 @@ Make sure of the following - you are using cyclone dds both on the robot and on the external pc -- you have the same `ROS_DOAMIN_ID` on both the PC and the raspberry-pi, to do so do the following both on raspberry-pi and on your PC +- you have the same `ROS_DOMAIN_ID` on both the PC and the raspberry-pi, to do so do the following both on raspberry-pi and on your PC ``` nano ~/.bashrc @@ -67,6 +64,6 @@ nano ~/.bashrc then add the following at the end of the file ``` -export ROS_DOAMIN_ID=38 +export ROS_DOMAIN_ID=38 ``` From 3f0df5610b9ea859ccc88a82d3168e9150ef911f Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Mon, 4 Aug 2025 08:15:40 +0200 Subject: [PATCH 38/40] fix base footprint velocity feedback --- .../include/gait_generator/gait_generator.hpp | 1 + .../src/gait_generator/gait_generator_core.cpp | 10 +++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/gait_generator/include/gait_generator/gait_generator.hpp b/gait_generator/include/gait_generator/gait_generator.hpp index 7d01450..52930f7 100644 --- a/gait_generator/include/gait_generator/gait_generator.hpp +++ b/gait_generator/include/gait_generator/gait_generator.hpp @@ -109,6 +109,7 @@ class GaitGenerator { void update_phase(double delta_t_milli); void update_feet_positions(double delta_t_milli); void publish_base_to_basefootprint_transform(); + void publish_base_footprint_vel_feedback(); // move feet up (z up) calculation double foot_pos_z_generator(double b, double q, double phase_shift, int n) { diff --git a/gait_generator/src/gait_generator/gait_generator_core.cpp b/gait_generator/src/gait_generator/gait_generator_core.cpp index 69c111b..ad016d4 100644 --- a/gait_generator/src/gait_generator/gait_generator_core.cpp +++ b/gait_generator/src/gait_generator/gait_generator_core.cpp @@ -224,10 +224,18 @@ void GaitGenerator::update_feet_positions(double delta_t_milli) { } } +void GaitGenerator::publish_base_footprint_vel_feedback() { + auto robot_vel = geometry_msgs::msg::Twist(); + robot_vel.linear.x = - cmd_vel_.linear.x; + robot_vel.linear.y = - cmd_vel_.linear.y; + robot_vel.angular.z = - cmd_vel_.angular.z; + feedback_cmd_vel_publisher_->publish(robot_vel); +} + void GaitGenerator::timer_callback(double delta_t_milli) { update_phase(delta_t_milli); update_feet_positions(delta_t_milli); - feedback_cmd_vel_publisher_->publish(cmd_vel_); + publish_base_footprint_vel_feedback(); } } // namespace penta_pod::kin::gait_generator From 4914860348c5a19892cd6b1b89e776215d5ebbe0 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Thu, 7 Aug 2025 23:18:39 +0200 Subject: [PATCH 39/40] inital joints angles --- penta_description/urdf/single_limb.urdf.xacro | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/penta_description/urdf/single_limb.urdf.xacro b/penta_description/urdf/single_limb.urdf.xacro index 5ccbf37..2880959 100644 --- a/penta_description/urdf/single_limb.urdf.xacro +++ b/penta_description/urdf/single_limb.urdf.xacro @@ -155,17 +155,23 @@ - + + 0.0 + - + + 1.57 + - + + 0.0 + From 6c997607fac0aabb2ccb39efc0c8e7bc0a4f6276 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Sun, 24 Aug 2025 16:08:39 +0200 Subject: [PATCH 40/40] fix var type --- pac9685_cpp/pca9685_interfaces/msg/PcaChannelParams.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pac9685_cpp/pca9685_interfaces/msg/PcaChannelParams.msg b/pac9685_cpp/pca9685_interfaces/msg/PcaChannelParams.msg index 85a8991..980c1c4 100644 --- a/pac9685_cpp/pca9685_interfaces/msg/PcaChannelParams.msg +++ b/pac9685_cpp/pca9685_interfaces/msg/PcaChannelParams.msg @@ -1,6 +1,6 @@ string name int32 pwm_channel -int32 min_pulse -int32 max_pulse +float64 min_pulse +float64 max_pulse float64 min_angle float64 max_angle \ No newline at end of file