Enable multiple robots

README.md

@@ -113,7 +113,7 @@ In this section we will install all the necessary dependencies in order to be ab
 - **ROS 2.0 Crystal**: following the official instructions, [source](https://index.ros.org/doc/ros2/Linux-Development-Setup/) or [debian packages](https://index.ros.org/doc/ros2/Linux-Install-Debians/).
 
 ### Dependent tools
-**Note**: We recommend installing **Gazebo 9.0.0** via **ROS Crystal Debian packages** and removing previous gazebo installations to avoid undesired conflicts, e.g. `apt-get remove *gazebo*`. You can also use different versions of the simulator such as Gazebo 10, but you must skip the installation of `ros-crystal-gazebo*` packages and add [gazebo_ros_pkgs](https://github.com/ros-simulation/gazebo_ros_pkgs/tree/crystal) to the `ros2_mara_ws` we are going to build in the [Create a ROS workspace](#create-a-ros-20-workspace) section.
+**Note**: We recommend installing **Gazebo 9.9.0** via **ROS Crystal Debian packages** and removing previous gazebo installations to avoid undesired conflicts, e.g. `apt-get remove *gazebo*`. You can also use different versions of the simulator such as Gazebo 10, but you must skip the installation of `ros-crystal-gazebo*` packages.
 
 ```sh
 # ROS 2 extra packages
@@ -214,13 +214,13 @@ source ~/ros2_mara_ws/install/setup.bash
 ros2 launch mara_gazebo mara.launch.py
 ```
 
-**Optionally**, you can launch the MARA robot with gripper and/or a table using the `--urdf` flag to indicate the desired urdf to be spawned:
+**Optionally**, you can launch a different versions of MARA robot using the `--urdf` flag to indicate the desired urdf to be spawned:
 
 ```sh
 ros2 launch mara_gazebo mara.launch.py --urdf mara_robot_gripper_140
 ```
 
-*Available urdfs: `mara_robot_gripper_140`, `mara_robot_gripper_140_no_table`, `mara_robot_gripper_85` and `mara_robot_gripper_hande`*
+*Available urdfs: `mara_robot_gripper_140`, `mara_robot_gripper_140_no_table`, `mara_robot_gripper_85`, `mara_robot_gripper_hande`, `two_mara_robots` and `two_mara_robots_gripper_140_no_table`*
 
 <br/>
 
@@ -255,27 +255,42 @@ source ~/catkin_mara_ws/devel_isolated/setup.bash
 roslaunch mara_bringup mara_bringup_moveit_actions.launch
 ```
 
-**Optionally**, you can launch one of these launch files, according to the choice in the Terminal 1.
+If you have used a different urdf in the Terminal 1, you will need to use `urdf:=` to launch the same one:
 
 ```sh
-roslaunch mara_bringup mara_bringup_moveit_actions.launch gripper:=true prefix:=140 table:=false
-roslaunch mara_bringup mara_bringup_moveit_actions.launch gripper:=true prefix:=140
-roslaunch mara_bringup mara_bringup_moveit_actions.launch gripper:=true prefix:=85
-roslaunch mara_bringup mara_bringup_moveit_actions.launch gripper:=true prefix:=hande
+roslaunch mara_bringup mara_bringup_moveit_actions.launch urdf:=mara_robot_gripper_140
 ```
 
 #### Terminal 3 (bridge)
+Source catkin_mara_ws and ros2_mara_ws:
 ```sh
 source ~/catkin_mara_ws/devel_isolated/setup.bash
 source ~/ros2_mara_ws/install/setup.bash
-
+```
+Run the bridge:
+```sh
 ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/motors.yaml sim
 ```
 
+If you have launched two mara robots, you will have to run the bridge in the following way:
+```sh
+ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/two_motors.yaml sim
+```
+
 ### MoveIt! with MARA - Real Robot
 Plan trajectories in a real environment with MoveIt!.
 
-:warning: You will need to change the names of the real motors in [MARA/hros_cognition_mara_components](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L10-L15) and in [MARA_ROS1/mara_bringup](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml#L10-L15) files to match the MACs of your SoMs.
+:warning: You will need to change the names of the real motors in [MARA/hros_cognition_mara_components](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L16-L21) and in [MARA_ROS1/mara_bringup](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml#L10-L15) files to match the MACs of your SoMs.
+
+:warning: Any change in the yaml files you will have to recompile the ros2 and ros packages (make sure you source only the corresponding ros/ros2):
+```sh
+source /opt/ros/crystal/setup.bash
+cd ~/ros2_mara_ws && colcon build --merge-install --packages-select hros_cognition_mara_components
+```
+```sh
+source /opt/ros/melodic/setup.bash
+cd ~/catkin_mara_ws && catkin_make_isolated --install --pkg mara_bringup
+```
 
 #### Terminal 1 (ROS 2.0)
 
@@ -288,13 +303,13 @@ export ROS_DOMAIN_ID=22
 ros2 launch mara_bringup mara.launch.py
 ```
 
-If your real robot includes a gripper, you will have to set the `--urdf` flag to indicate the urdf that contains the gripper your robot has:
+If your real robot has any extra component or you want to control more than one robot, you will need to set the `--urdf` flag to indicate the urdf that corresponds to your real robot (environment):
 
 ```sh
 ros2 launch mara_bringup mara.launch.py --urdf mara_robot_gripper_140
 ```
 
-*Available urdfs: `mara_robot_gripper_140`, `mara_robot_gripper_140_no_table`, `mara_robot_gripper_85` and `mara_robot_gripper_hande`*
+*Available urdfs: `mara_robot_gripper_140`, `mara_robot_gripper_140_no_table`, `mara_robot_gripper_85`, `mara_robot_gripper_hande`, `two_mara_robots` and `two_mara_robots_gripper_140_no_table`*
 
 #### Terminal 2 (ROS)
 
@@ -303,26 +318,34 @@ source ~/catkin_mara_ws/devel_isolated/setup.bash
 roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real
 ```
 
-If you have used a different urdf in the Terminal 1, you will have to launch the corresponding one to match it:
+If you have used a different urdf in the Terminal 1, you will need to use `urdf:=` to launch the same one:
 
 ```sh
-roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real gripper:=true prefix:=140 table:=false
-roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real gripper:=true prefix:=140
-roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real gripper:=true prefix:=85
-roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real gripper:=true prefix:=hande
+roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real urdf:=mara_robot_gripper_140
 ```
 
+*If case you want to control two robots you will need to add `multiple_robots:=true`*
+
+
 #### Terminal 3 (bridge)
 
+Source catkin_mara_ws nad ros2_mara_ws, and export RMW_IMPLEMENTATION and ROS_DOMAIN_ID:
 ```sh
 source ~/catkin_mara_ws/devel_isolated/setup.bash
 source ~/ros2_mara_ws/install/setup.bash
 # you will need to change the export values according to the SoMs configuration, same as in Terminal 1
 export RMW_IMPLEMENTATION=rmw_opensplice_cpp
 export ROS_DOMAIN_ID=22
-
+```
+Run the bridge:
+```sh
 ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/motors.yaml real
 ```
+If you have two mara robots, you will have to run the bridge in the following way:
+```sh
+ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/two_motors.yaml real
+```
+
 <br/>
 
 ## Examples

hros_cognition_mara_components/CMakeLists.txt

@@ -62,6 +62,7 @@ install(  TARGETS
 install(
   PROGRAMS
   config/motors.yaml
+  config/two_motors.yaml
   DESTINATION share/${PROJECT_NAME}
 )
 

hros_cognition_mara_components/config/motors.yaml

@@ -13,3 +13,11 @@ real_motors:
   - "/hrim_actuation_servomotor_70B3D521A08A/trajectory_axis2"
   - "/hrim_actuation_servomotor_70B3D521A0A4/trajectory_axis1"
   - "/hrim_actuation_servomotor_70B3D521A0A4/trajectory_axis2"
+
+motors:
+  - "motor1"
+  - "motor2"
+  - "motor3"
+  - "motor4"
+  - "motor5"
+  - "motor6"

hros_cognition_mara_components/config/two_motors.yaml

@@ -0,0 +1,41 @@
+simulated_motors:
+  - "/hrim_actuation_servomotor_000000000001/trajectory_axis2"
+  - "/hrim_actuation_servomotor_000000000001/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000002/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000002/trajectory_axis2"
+  - "/hrim_actuation_servomotor_000000000003/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000003/trajectory_axis2"
+  - "/hrim_actuation_servomotor_000000000005/trajectory_axis2"
+  - "/hrim_actuation_servomotor_000000000005/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000006/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000006/trajectory_axis2"
+  - "/hrim_actuation_servomotor_000000000007/trajectory_axis1"
+  - "/hrim_actuation_servomotor_000000000007/trajectory_axis2"
+
+real_motors:
+  - "/hrim_actuation_servomotor_70B3D521A042/trajectory_axis2"
+  - "/hrim_actuation_servomotor_70B3D521A042/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A08A/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A08A/trajectory_axis2"
+  - "/hrim_actuation_servomotor_70B3D521A0A4/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A0A4/trajectory_axis2"
+  - "/hrim_actuation_servomotor_70B3D521A084/trajectory_axis2"
+  - "/hrim_actuation_servomotor_70B3D521A084/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A0C0/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A0C0/trajectory_axis2"
+  - "/hrim_actuation_servomotor_70B3D521A092/trajectory_axis1"
+  - "/hrim_actuation_servomotor_70B3D521A092/trajectory_axis2"
+
+motors:
+  - "motor1"
+  - "motor2"
+  - "motor3"
+  - "motor4"
+  - "motor5"
+  - "motor6"
+  - "mara2_motor1"
+  - "mara2_motor2"
+  - "mara2_motor3"
+  - "mara2_motor4"
+  - "mara2_motor5"
+  - "mara2_motor6"

hros_cognition_mara_components/include/HROSCognitionMaraComponents.hpp

@@ -21,69 +21,70 @@ using namespace std::placeholders;
 class HROSCognitionMaraComponentsNode : public rclcpp::Node
 {
   public:
-  /// LifecycleTalker constructor
-  /**
-   * The lifecycletalker/lifecyclenode constructor has the same
-   * arguments a regular node.
-   */
-  explicit HROSCognitionMaraComponentsNode(const std::string & node_name,
-                      int argc, char **argv, bool intra_process_comms = false);
-
-  /// Callback for walltimer in order to publish the message.
-  /**
-   * Callback for walltimer. This function gets invoked by the timer
-   * and executes the publishing.
-   * For this demo, we ask the node for its current state. If the
-   * lifecycle publisher is not activate, we still invoke publish, but
-   * the communication is blocked so that no messages is actually transferred.
-   */
-   void timer_stateCommonPublisher();
-   void timer_commandPublisher();
-
-   std::thread* the_thread;
-   std::vector<std::string> node_names_;
-   std::mutex mutex_node_names;
-
-private:
-  rclcpp::Clock clock_ros;
-  // We hold an instance of a lifecycle publisher. This lifecycle publisher
-  // can be activated or deactivated regarding on which state the lifecycle node
-  // is in.
-
-  // We hold an instance of a timer which periodically triggers the publish function.
-  // As for the beta version, this is a regular timer. In a future version, a
-  // lifecycle timer will be created which obeys the same lifecycle management as the
-  // lifecycle publisher.
-  std::shared_ptr<rclcpp::TimerBase> timer_command_;
-
-  std::string node_name;
-
-  std::vector<std::shared_ptr<rclcpp::Subscription<hrim_actuator_rotaryservo_msgs::msg::StateRotaryServo>>> motor_state_subcriptions_;
-  std::vector<std::shared_ptr<rclcpp::Publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>>> motor_goal_publishers_;
-
-	std::shared_ptr<rclcpp::Publisher<control_msgs::msg::JointTrajectoryControllerState>> common_joints_pub_;
-  std::shared_ptr<rclcpp::TimerBase> timer_common_joints_;
-
-  std::vector<std::shared_ptr<rclcpp::Publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>>> list_available_publishers;
-
-	std::shared_ptr<rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>> trajectory_sub_;
-  void commandCallback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg);
-  void stateCallback(std::string motor_name, float velocity, float position, float effort);
-
-  std::vector<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo> cmd_to_send;
-
-  pthread_mutex_t mutex_command;
-
-  double nan;
-
-  pthread_mutex_t mtx;
-	control_msgs::msg::JointTrajectoryControllerState msg_actuators_;
-
-  std::string file_motors;
-  std::string environment;
-  std::string motor_key;
-
-  std::vector<bool> msg_actuators_callback_sync;
+    /// LifecycleTalker constructor
+    /**
+     * The lifecycletalker/lifecyclenode constructor has the same
+     * arguments a regular node.
+     */
+    explicit HROSCognitionMaraComponentsNode(const std::string & node_name,
+                        int argc, char **argv, bool intra_process_comms = false);
+
+    /// Callback for walltimer in order to publish the message.
+    /**
+     * Callback for walltimer. This function gets invoked by the timer
+     * and executes the publishing.
+     * For this demo, we ask the node for its current state. If the
+     * lifecycle publisher is not activate, we still invoke publish, but
+     * the communication is blocked so that no messages is actually transferred.
+     */
+     void timer_stateCommonPublisher();
+     void timer_commandPublisher();
+
+     std::thread* the_thread;
+     std::vector<std::string> node_names_;
+     std::mutex mutex_node_names;
+
+  private:
+    rclcpp::Clock clock_ros;
+    // We hold an instance of a lifecycle publisher. This lifecycle publisher
+    // can be activated or deactivated regarding on which state the lifecycle node
+    // is in.
+
+    // We hold an instance of a timer which periodically triggers the publish function.
+    // As for the beta version, this is a regular timer. In a future version, a
+    // lifecycle timer will be created which obeys the same lifecycle management as the
+    // lifecycle publisher.
+    std::shared_ptr<rclcpp::TimerBase> timer_command_;
+
+    std::string node_name;
+
+    std::vector<std::shared_ptr<rclcpp::Subscription<hrim_actuator_rotaryservo_msgs::msg::StateRotaryServo>>> motor_state_subcriptions_;
+    std::vector<std::shared_ptr<rclcpp::Publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>>> motor_goal_publishers_;
+
+  	std::shared_ptr<rclcpp::Publisher<control_msgs::msg::JointTrajectoryControllerState>> common_joints_pub_;
+    std::shared_ptr<rclcpp::TimerBase> timer_common_joints_;
+
+    std::vector<std::shared_ptr<rclcpp::Publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>>> list_available_publishers;
+
+  	std::shared_ptr<rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>> trajectory_sub_;
+    void commandCallback(const trajectory_msgs::msg::JointTrajectory::SharedPtr msg);
+    void stateCallback(std::string motor_name, float velocity, float position, float effort);
+
+    std::vector<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo> cmd_to_send;
+
+    pthread_mutex_t mutex_command;
+
+    double nan;
+
+    pthread_mutex_t mtx;
+  	control_msgs::msg::JointTrajectoryControllerState msg_actuators_;
+
+    std::string file_motors;
+    std::string environment;
+    std::string motor_key;
+
+    std::vector<std::string> motor_names;
+    std::vector<bool> msg_actuators_callback_sync;
 
 };