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/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/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/include/gait_generator/gait_generator.hpp b/gait_generator/include/gait_generator/gait_generator.hpp index d9040b7..52930f7 100644 --- a/gait_generator/include/gait_generator/gait_generator.hpp +++ b/gait_generator/include/gait_generator/gait_generator.hpp @@ -90,6 +90,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_{}; @@ -107,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/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/gait_generator/src/gait_generator/base_tf_broadcaster.cpp b/gait_generator/src/gait_generator/base_tf_broadcaster.cpp index 819ee20..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); @@ -18,8 +22,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 +38,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 diff --git a/gait_generator/src/gait_generator/gait_generator_core.cpp b/gait_generator/src/gait_generator/gait_generator_core.cpp index de47488..ad016d4 100644 --- a/gait_generator/src/gait_generator/gait_generator_core.cpp +++ b/gait_generator/src/gait_generator/gait_generator_core.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); create_set_gait_pattern_service(); } @@ -221,9 +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); + publish_base_footprint_vel_feedback(); } } // namespace penta_pod::kin::gait_generator 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/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 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..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_; - std::vector joint_positions_; - std::vector joint_velocities_; - 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_; 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..c43a60b 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,10 +78,22 @@ 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_efforts_ = 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( + 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 + actuator_position_commands_rad_[i] = angle_rad; // Store in radians + actuator_positions_feedback_rad_[i] = angle_rad; // Initialize positions with initial angles + } + } + 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) { @@ -138,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_[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_[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; @@ -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, &actuator_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_actuator_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]; + actuator_positions_feedback_rad_[index] = actuator_position_commands_rad_[index]; } // defferintiate velocities - if (previous_joint_position_commands_.empty()) { - previous_joint_position_commands_ = joint_position_commands_; + 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_[index] = (joint_position_commands_[index] - previous_joint_position_commands_[index]) / period.seconds(); - previous_joint_position_commands_[index] = joint_position_commands_[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 @@ -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 = actuator_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/config/penta.rviz b/penta_description/config/penta.rviz index d434855..948bc53 100644 --- a/penta_description/config/penta.rviz +++ b/penta_description/config/penta.rviz @@ -5,8 +5,9 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 + - /Map1 Splitter Ratio: 0.5 - Tree Height: 296 + Tree Height: 617 - Class: rviz_common/Views Expanded: - /Current View1 @@ -65,6 +66,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb0_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb0_lower_arm_link: Alpha: 1 Show Axes: false @@ -85,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 @@ -105,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 @@ -125,6 +141,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb3_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb3_lower_arm_link: Alpha: 1 Show Axes: false @@ -145,6 +166,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + limb4_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true limb4_lower_arm_link: Alpha: 1 Show Axes: false @@ -165,6 +191,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + lower_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upper_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -187,6 +223,61 @@ Visualization Manager: {} 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 @@ -202,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 diff --git a/penta_description/launch/robot_state_publisher.launch.py b/penta_description/launch/robot_state_publisher.launch.py index 9b62822..416bce3 100644 --- a/penta_description/launch/robot_state_publisher.launch.py +++ b/penta_description/launch/robot_state_publisher.launch.py @@ -1,51 +1,43 @@ 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="", 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', 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'), + ' use_gazebo_simulation:=', + LaunchConfiguration('use_gazebo_simulation'), + ]) + return LaunchDescription([ name_space_arg, + use_gazebo_simulation_arg, remapping_arg, Node( package='robot_state_publisher', @@ -54,7 +46,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/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 + + + + + + + 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_description/urdf/penta.urdf.xacro b/penta_description/urdf/penta.urdf.xacro index 0c13b13..d6b802f 100644 --- a/penta_description/urdf/penta.urdf.xacro +++ b/penta_description/urdf/penta.urdf.xacro @@ -2,6 +2,7 @@ + @@ -23,9 +24,11 @@ - + - + + + @@ -46,13 +49,13 @@ - - - + + + - + @@ -61,14 +64,16 @@ - - - + + + - - + + + + @@ -77,22 +82,22 @@ - - - + + + + + + + + + - - - - - - - - + + @@ -105,7 +110,7 @@ - + diff --git a/penta_description/urdf/single_limb.urdf.xacro b/penta_description/urdf/single_limb.urdf.xacro index 171c57b..2880959 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,94 +111,100 @@ - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - + gazebo_ros2_control/GazeboSystem - + - + + 0.0 + - + - + + 1.57 + - + - + + 0.0 + - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + hardware_interface/PositionJointInterface 1 - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + hardware_interface/PositionJointInterface 1 - + transmission_interface/SimpleTransmission - + hardware_interface/PositionJointInterface - + hardware_interface/PositionJointInterface 1 diff --git a/penta_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={ 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 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 diff --git a/penta_pod/launch/nav2/README.md b/penta_pod/launch/nav2/README.md new file mode 100644 index 0000000..8fe391f --- /dev/null +++ b/penta_pod/launch/nav2/README.md @@ -0,0 +1,69 @@ +# About + +Setting up autonomous navigation for the penta_pod robot, + +Hardware requirement: + +- pentapod robot connected to wifi + +- 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 installation on the robot + +To install software on the robot raspberry-pi, from inside the folder [scripts](./scripts/) run the installation script: + +``` +./install_rplidar_on_robot.sh +``` + +# Software installation on PC + +To install navigation stack NAV2 on PC, from inside the folder [scripts](./scripts/) run the installation script: + +``` +./install_nav2_on_pc.sh +``` + +# What to launch on the robot + +Turn on the robot hardware using: + +``` +ros2 launch penta_pod realhardware_bringup.launch.py +``` + + +# What to launch on an external PC + + +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 +``` + + +# Extra considerations + +Make sure of the following + +- your PC and the robot are connected on same wifi + +- you are using cyclone dds both on the robot and on the external 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 +``` + +then add the following at the end of the file + +``` +export ROS_DOMAIN_ID=38 +``` + 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..072259e --- /dev/null +++ b/penta_pod/launch/nav2/nav2_bringup.launch.py @@ -0,0 +1,110 @@ +import os + +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 + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +# 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", + ) + # 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( + [ + os.path.join( + 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") + + 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", + ) + ] + ) + ) + + # 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 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"), + ], + ), + ]) 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/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..dd4be2a --- /dev/null +++ b/penta_pod/launch/nav2/scripts/install_rplidar_on_robot.sh @@ -0,0 +1,21 @@ +#! /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 --packages-select rplidar_ros +colcon build --packages-up-to penta_pod --parallel-workers 1 \ 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 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( 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, ]) 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, ]) 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..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 @@ -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", ) @@ -96,10 +99,11 @@ def generate_launch_description(): forward_controller_spawner = Node( package="controller_manager", executable="spawner", + namespace=LaunchConfiguration("name_space"), arguments=[ "forward_position_controller", "--controller-manager", - "/controller_manager", + "controller_manager", ], output="screen", ) 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/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} ) 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/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/forward_joint_command_bridge.launch.py b/ros_to_ros2_control_command_bridge/launch/forward_joint_command_bridge.launch.py index 5b412d6..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")}, 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/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; 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/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..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,26 +2,26 @@ 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 - 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); }); } -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; @@ -30,14 +30,15 @@ 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); } -void GazeboForwardJointCommandControlBridge::declareParameters() +void ForwardJointCommandControlBridge::declareParameters() { node_->declare_parameter>("joint_angles_at_initial_pose_degree", std::vector{} @@ -51,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)) { 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 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; +}