Allow running several instances with rviz and gazebo in different name spaces

.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
+

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
+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
+```

gait_generator/include/gait_generator/base_tf_broadcaster.hpp

@@ -19,6 +19,10 @@ class BaseTfBroadcaster {
   rclcpp::Node::SharedPtr node_;
   rclcpp::Subscription<PoseStamped>::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();

gait_generator/include/gait_generator/gait_generator.hpp

@@ -90,6 +90,8 @@ class GaitGenerator {
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
       cmd_null_pos_subscription_; // null space motion transform (body to
                                   // basefootprint)
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::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) {

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

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<PoseStamped>(
       "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

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<int>(delta_t_milli)),
       [this, delta_t_milli]() { timer_callback(delta_t_milli); });
+
+  feedback_cmd_vel_publisher_ = node_->create_publisher<geometry_msgs::msg::Twist>(
+      "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

joints_aggregator/include/joints_aggregator/joints_aggregator.hpp

@@ -31,6 +31,7 @@ class JointsAggregator {
   std::mutex q_mutex_;
   std::vector<double> q_;
   std::vector<double> previous_q_;
+  std::string name_space_;
   int limbs_num_;
   std::vector<long int> joints_per_limb_;
   int update_interval_millis_;

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"),
                 ],

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<std::string>("name_space", "");
   node_->declare_parameter<int>("limbs_num");
   node_->declare_parameter<std::vector<long int>>("joints_per_limb",
                                                   std::vector<long int>{});
@@ -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(

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

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<double> joint_position_commands_;
-  std::vector<double> joint_positions_;
-  std::vector<double> joint_velocities_;
-  std::vector<double> joint_efforts_;
+  std::vector<double> actuator_position_commands_rad_;
+  std::vector<double> actuator_positions_feedback_rad_;
+  std::vector<double> actuator_velocities_rad_per_sec_;
+  std::vector<double> actuator_efforts_;
 
   // Motors parameters
   std::string i2c_device_name_;

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<long>(min_angle_string);
   std::string max_angle_string  = info_.hardware_parameters.at("motors.max_joint_angle_for_each_channel");
   auto max_angle = parse_list<long>(max_angle_string);
+  auto init_angles = parse_list<long>(info_.hardware_parameters.at("motors.init_angles"));
 
   if (min_pulse.size() != static_cast<size_t>(number_of_motors_) ||
       max_pulse.size() != static_cast<size_t>(number_of_motors_) ||
       min_angle.size() != static_cast<size_t>(number_of_motors_) ||
-      max_angle.size() != static_cast<size_t>(number_of_motors_))
+      max_angle.size() != static_cast<size_t>(number_of_motors_) ||
+      init_angles.size() != static_cast<size_t>(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<double>(number_of_motors_, 0.);
-  joint_positions_ = std::vector<double>(number_of_motors_, 0.);
-  joint_velocities_ = std::vector<double>(number_of_motors_, 0.);
-  joint_efforts_ = std::vector<double>(number_of_motors_, 0.);
+  actuator_position_commands_rad_ = std::vector<double>(number_of_motors_, 0.);
+  actuator_positions_feedback_rad_ = std::vector<double>(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<double>(number_of_motors_, 0.);
+  actuator_efforts_ = std::vector<double>(number_of_motors_, 0.);
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
@@ -138,13 +152,13 @@ std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> 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<double> previous_joint_position_commands_;
+  static std::vector<double> 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();