Add stl

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

.devcontainer/docker-compose.yaml

@@ -7,10 +7,20 @@ services:
         COLCON_WORKSPACE: /root/ros_ws
     volumes:
       - ../:/root/ros_ws/src
+      - /dev:/dev
+      - /tmp/.X11-unix:/tmp/.X11-unix
     environment:
       - ROS_DOMAIN_ID=91
-    network_mode: "host"
-    command: ["/bin/bash", "-c", "/root/ros_ws/src/.devcontainer/entrypoint.sh"]
+      - DISPLAY=${DISPLAY}
+      - QT_X11_NO_MITSHM=1
+    privileged: true
+    devices:
+      - "/dev:/dev"
+      - /dev/bus/usb:/dev/bus/usb
+      - /dev/i2c-0:/dev/i2c-0
+      - /dev/i2c-1:/dev/i2c-1
+    tty: true
+    command: [ "/bin/bash", "-c", "/root/ros_ws/src/.devcontainer/entrypoint.sh" ]
 
   scorpion-ros2-test:
     build:
@@ -23,4 +33,4 @@ services:
     environment:
       - ROS_DOMAIN_ID=91
     network_mode: "host"
-    command: ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash && cd /root/ros_ws && colcon test"]
+    command: [ "/bin/bash", "-c", "source /opt/ros/humble/setup.bash && cd /root/ros_ws && colcon test" ]

README.md

@@ -1,7 +1,7 @@
 [![Powered by Dev Containers](https://img.shields.io/badge/Powered%20by-Dev%20Containers-blue?logo=devcontainers&logoColor=white)](https://containers.dev/)
 
 
-# Scorpion
+# Pentapod
 
 This is a ROS2 software for the following older project [youtube video available here.](https://youtu.be/kcvJR5mcb1o?si=lxt_06UO4189CPcX)
 
@@ -21,17 +21,17 @@ cd ~/my_ws/src
 Clone the repo
 
 ```
-git clone git@github.com:Modi1987/scorpion.git
+git clone git@github.com:Modi1987/pentapod.git
 
-cd scorpion
+cd pentapod
 
 git checkout i2c_actuators
 ```
 
 There are bash scripts that will allow you to build your workspace you can do this on your PC if you want to run RVIZ simulations, or on Raspberry-pi4 if you want to run on real-robot
 
 ```
-cd ~/my_ws/src/scorpion/scripts
+cd ~/my_ws/src/pentapod/scripts
 
 ./setup_penta_workspace.sh
 ```
@@ -41,7 +41,7 @@ cd ~/my_ws/src/scorpion/scripts
 besides to the previous steps to setup your workspace, to control the real-robot you will need to configure the i2c bus on Raspberry-pi 4, to do so on the Raspberry-pi 4:
 
 ```
-cd ~/my_ws/src/scorpion/scripts
+cd ~/my_ws/src/pentapod/scripts
 
 ./setup_i2c_on_raspberry_pi4.sh
 ```
@@ -64,22 +64,30 @@ Finally, you can move the robot around from external PC using keyboard
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
 ```
 
+## If you are using Hiwonder motors over serial bus
+
+'''
+ros2 launch penta_pod realhardware_bringup.launch.py motors_interface:=hiwonder
+'''
+
 ## Setup as a service (automatic on boot)
 
 To bring-up the robot automatically each time you boot the robot (without having to launch the brinup manually), run the following script on the Raspberry Pi
 
 ```
-cd ~/my_ws/src/scorpion/scripts
+cd ~/my_ws/src/pentapod/scripts
 
 ./setup_service_penta_bringup_on_boot.sh
 ```
 
 ## Run in simulation
 
+### Using Rviz
+
 You can run the simulation using the command
 
 ```
-ros2 launch penta_pod penta_simn_rviz.launch.py
+ros2 launch penta_pod penta_sim_rviz.launch.py
 ```
 
 You can move the robot around using
@@ -88,6 +96,13 @@ You can move the robot around using
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
 ```
 
+### Gazebo simulation
+
+You can also run a gazebo simulation using
+
+```
+ros2 launch penta_gazebo_sim penta_sim_full_gazebo.launch.py
+```
 
 ## Using devcontainers
 
@@ -131,4 +146,66 @@ cd ..
 source install/setup.bash
 
 ros2 launch penta_pod penta_pod.launch.py
-```
+```
+
+
+# Swarm of robots on same network
+
+You can create a swarm of the real robot, all of them connected to eachothers sharing the same network, each robot will be distnguished by its own namespace as the following:
+
+For 'robot1'
+
+```
+ros2 launch penta_pod realhardware_bringup.launch.py name_space:=robot1 mode:=real
+
+```
+
+For 'robot2'
+
+```
+ros2 launch penta_pod realhardware_bringup.launch.py name_space:=robot2 mode:=real
+
+```
+
+You can check that the topics are in the namespace, same for services and actions, you can use rqt introspection to show the ndoes graph.
+
+```
+~/ros_ws# ros2 topic list
+/parameter_events
+/robot1/actuator_setpoint_degree
+/robot1/cmd_vel
+/robot1/joint_setpoints
+/robot1/joy
+/robot1/joy/set_feedback
+/robot1/limb0/joint_setpoints
+/robot1/limb0/xyz_msg
+/robot1/limb1/joint_setpoints
+/robot1/limb1/xyz_msg
+/robot1/limb2/joint_setpoints
+/robot1/limb2/xyz_msg
+/robot1/limb3/joint_setpoints
+/robot1/limb3/xyz_msg
+/robot1/limb4/joint_setpoints
+/robot1/limb4/xyz_msg
+/robot1/null_space_pose
+/robot2/actuator_setpoint_degree
+/robot2/cmd_vel
+/robot2/joint_setpoints
+/robot2/joy
+/robot2/joy/set_feedback
+/robot2/limb0/joint_setpoints
+/robot2/limb0/xyz_msg
+/robot2/limb1/joint_setpoints
+/robot2/limb1/xyz_msg
+/robot2/limb2/joint_setpoints
+/robot2/limb2/xyz_msg
+/robot2/limb3/joint_setpoints
+/robot2/limb3/xyz_msg
+/robot2/limb4/joint_setpoints
+/robot2/limb4/xyz_msg
+/robot2/null_space_pose
+/rosout
+/tf
+```
+
+You can publish on `/robot1/cmd_vel` to control first robot, and on `robot2/cmd_vel` to control second robot and so forth. If you do not have real robot you can use `mode:=virtual` and test

base_twerk/include/base_twerk/base_twerk_action_server.hpp

@@ -44,6 +44,8 @@ class BaseTwerkActionServer {
 
   auto execute(const std::shared_ptr<GoalHandle> goal_handle) -> void;
 
+  auto is_target_valid(const std::shared_ptr<GoalHandle> goal_handle) -> bool;
+
   auto declare_parameters() -> void;
   auto get_parameters() -> void;
 
@@ -52,11 +54,16 @@ class BaseTwerkActionServer {
   auto calculate_twerk_pose_from_goal(
       rclcpp::Time start_time, PoseStamped start_pose,
       const std::shared_ptr<GoalHandle> goal_handle) -> PoseStamped;
-      
+
   auto quiry_current_base_pose() -> std::optional<GetCurrentBasePose::Response>;
 
-  double update_interval_millis_double_;
-  double max_permissible_displacement_meter_;
+  struct TwerkYamlConfigs {
+    double update_interval_millis_double_;
+    double max_permissible_displacement_meter_;
+    double min_permissible_displacement_meter_;
+    double max_permissible_rotation_rad_;
+    double min_permissible_rotation_rad_;
+  } twerk_yaml_configs_;
 
   struct ReceviedPoseStamped {
     rclcpp::Time timestamp;

base_twerk/include/base_twerk/base_twerk_helper_funs.hpp

@@ -0,0 +1,25 @@
+#ifndef BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_
+#define BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_
+
+#include "math.h"
+
+auto get_rounded_dance_time_from_goal_millis(double w, int dance_time_millis)
+    -> std::chrono::milliseconds {
+
+  constexpr double to_millis = 1000.0;
+
+  auto dance_period_seconds = 2 * M_PI / w;
+  auto dance_time_seconds = static_cast<double>(dance_time_millis) / to_millis;
+
+  auto num_periods = dance_time_seconds / dance_period_seconds;
+
+  auto rounded_periods =
+      std::round(num_periods) + 1; // +1 to ensure at least one period
+
+  auto rounded_dance_time_millis =
+      static_cast<long>(rounded_periods * dance_period_seconds * to_millis);
+
+  return std::chrono::milliseconds(rounded_dance_time_millis);
+}
+
+#endif // BASE_TWERK_BASE_TWERK_HELPER_FUNS_HPP_

base_twerk/launch/base_twerk_action_server.launch.py

@@ -2,21 +2,41 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
+
+    ld = LaunchDescription()
+
+    """ Launch args """
+    name_space_arg = DeclareLaunchArgument(
+        "name_space",
+        default_value="",
+        description="Robot name space",
+    )
+    ld.add_action(name_space_arg)
+
+    """ Nodes """
     config = os.path.join(
         get_package_share_directory("penta_description"),
         "config",
         "general_config.yaml",
     )
-    ld = LaunchDescription()
     ld.add_action(
         Node(
             package="base_twerk",
             executable="base_twerk_action_server_node",
             output="screen",
+            namespace=LaunchConfiguration("name_space"),
             parameters=[config],
+            remappings=[
+                # services
+                ("cmd_null_setpoint", "base_twerk/cmd_null_setpoint"),
+                ("get_current_null_pose", "base_twerk/get_current_null_pose"),
+                # topics
+            ],
         )
     )
     return ld

base_twerk/launch/null_pose_publisher.launch.py

@@ -2,21 +2,42 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
+
+    ld = LaunchDescription()
+
+    """ Launch args """
+    name_space_arg = DeclareLaunchArgument(
+        "name_space",
+        default_value="",
+        description="Robot name space",
+    )
+    ld.add_action(name_space_arg)
+
+    """ Nodes """
     config = os.path.join(
         get_package_share_directory("penta_description"),
         "config",
         "general_config.yaml",
     )
-    ld = LaunchDescription()
     ld.add_action(
         Node(
             package="base_twerk",
             executable="base_twerk_publisher_node",
             output="screen",
+            namespace=LaunchConfiguration("name_space"),
             parameters=[config],
+            remappings=[
+                # services
+                ("cmd_null_setpoint", "base_twerk/cmd_null_setpoint"),
+                ("get_current_null_pose", "base_twerk/get_current_null_pose"),
+                # topics
+                ("null_space_pose", "null_space_pose")
+            ],
         )
     )
     return ld