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,15 @@ 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`*
+
+*In case you use two mara robots, you will need to add extra [simulated_motors](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L8-L13) and [motors](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L30-L35), and compile the ros2 package again (make sure you only source crystal): `cd ~/ros2_mara_ws && colcon build --merge-install --packages-select hros_cognition_mara_components`.*
 
 <br/>
 
@@ -255,15 +257,14 @@ 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
 ```
 
+*In case you have used two mara robots, you will need to add extra [simulated_motors](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml#L8-L13) and compile the ros package again (make sure you only source melodic): `cd ~/catkin_mara_ws && catkin_make_isolated --install --pkg mara_bringup`*
+
 #### Terminal 3 (bridge)
 ```sh
 source ~/catkin_mara_ws/devel_isolated/setup.bash
@@ -275,7 +276,19 @@ ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -
 ### 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.
+
+In case you want to control two real robots you will need to add the new real_motors of the new robot in [ros2_mara_ws](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L15) and [catkin_mara_ws](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml#L15), and add extra [motors](https://github.com/AcutronicRobotics/MARA/blob/master/hros_cognition_mara_components/config/motors.yaml#L30-L35).
+
+: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 +301,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,15 +316,15 @@ 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)
 
 ```sh

hros_cognition_mara_components/config/motors.yaml

@@ -5,6 +5,12 @@ simulated_motors:
   - "/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"
@@ -13,3 +19,17 @@ 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"
+  # - "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;
 
 };
 

hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp

@@ -1,22 +1,16 @@
 #include "HROSCognitionMaraComponents.hpp"
 
-HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::string & node_name,
-                   int argc, char **argv, bool intra_process_comms )
-: rclcpp::Node(node_name, "", intra_process_comms)
+HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::string & node_name, int argc, char **argv, bool intra_process_comms): rclcpp::Node(node_name, "", intra_process_comms)
 {
+  RCUTILS_LOG_INFO_NAMED(get_name(), "HROSCognitionMaraComponentsNode::on_configure() is called.");
 
   this->node_name = node_name;
 
   auto qos_state = rmw_qos_profile_sensor_data;
   qos_state.depth = 1;
-  common_joints_pub_ = create_publisher<control_msgs::msg::JointTrajectoryControllerState>(
-    		 "/mara_controller/state",
-         qos_state);
+  common_joints_pub_ = create_publisher<control_msgs::msg::JointTrajectoryControllerState>("/mara_controller/state", qos_state);
 
-  trajectory_sub_ = create_subscription<trajectory_msgs::msg::JointTrajectory>(
-         "/mara_controller/command",
-         std::bind(&HROSCognitionMaraComponentsNode::commandCallback, this, _1),
-         rmw_qos_profile_sensor_data);
+  trajectory_sub_ = create_subscription<trajectory_msgs::msg::JointTrajectory>("/mara_controller/command", std::bind(&HROSCognitionMaraComponentsNode::commandCallback, this, _1), rmw_qos_profile_sensor_data);
 
   if (rcutils_cli_option_exist(argv, argv + argc, "-motors")){
     file_motors = std::string(rcutils_cli_get_option(argv, argv + argc, "-motors"));
@@ -26,19 +20,13 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
   pthread_mutex_init(&mutex_command, NULL);
 
   nan = std::numeric_limits<float>::quiet_NaN();
-
-  RCUTILS_LOG_INFO_NAMED(get_name(), "HROSCognitionMaraComponentsNode::on_configure() is called.");
-
 	std::vector<std::string> node_names;
-
-  std::cout << "===================== Reading link order ========================" << std::endl;
   std::vector<std::string> topic_order;
-  std::cout << "Trying to open  " << file_motors << std::endl;
 
   YAML::Node config = YAML::LoadFile(file_motors);
   if(config.IsNull()){
-      std::cout << "Not able to open the file" << std::endl;
-      return;
+    std::cout << "Not able to open: " << file_motors << std::endl;
+    return;
   }
   std::vector<std::string> lista_subcribers;
 
@@ -48,14 +36,19 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
   else if (environment == "real")
     motor_key = "real_motors";
 
+  std::cout << "====================== Reading link order ======================" << std::endl;
   for (auto motor : config[motor_key]) {
     std::string s = motor.as<std::string>();
     topic_order.push_back(s);
     std::cout << "topic name: " << s << std::endl;
   }
-  std::cout << "=====================================================" << std::endl;
-
-  std::cout << "++++++++++++++++++ Subscribers and Publishers++++++++++++++++++" << std::endl;
+  std::cout << "====================== Name of the motors ======================" << std::endl;
+  for(auto ms : config["motors"]){
+    std::string m = ms.as<std::string>();
+    motor_names.push_back(m);
+    std::cout << "motor_names: " << m << std::endl;
+  }
+  std::cout << "====================== Subscribers and Publishers ======================" << std::endl;
   for(unsigned int i = 0; i < topic_order.size(); i++){
 
     std::string topic = topic_order[i];
@@ -79,33 +72,24 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
     std::cout << "New publisher at " << motor_name << std::endl;
   }
 
-  std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
-
   msg_actuators_.actual.positions.resize(motor_goal_publishers_.size());
   msg_actuators_.actual.velocities.resize(motor_goal_publishers_.size());
   msg_actuators_.actual.effort.resize(motor_goal_publishers_.size());
   msg_actuators_.joint_names.resize(motor_goal_publishers_.size());
 
   for(unsigned int i = 0; i < topic_order.size(); i++){
-
     std::string topic = topic_order[i];
     std::string delimiter = "trajectory";
     std::string id = topic.substr( 0, topic.find(delimiter) );
     std::string axis = topic.erase( 0, topic.find(delimiter) + delimiter.length() );
     std::string motor_name = id + "state" + axis;
 
-    msg_actuators_.joint_names[i] =  motor_name;
-
-    std::cout << "motor_name: " << motor_name << std::endl;
+    msg_actuators_.joint_names[i] = motor_name;
   }
 
-  std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
-  std::cout << std::endl << std::endl;
-
   msg_actuators_callback_sync.resize(topic_order.size());
   for(unsigned int i = 0; i < msg_actuators_callback_sync.size(); i++){
     msg_actuators_callback_sync[i] =  false;
   }
-
   RCUTILS_LOG_INFO_NAMED(get_name(), "HROSCognitionMaraComponentsNode::on_configure() is finished.");
 }

hros_cognition_mara_components/src/TopicState.cpp

@@ -29,9 +29,8 @@ void HROSCognitionMaraComponentsNode::timer_stateCommonPublisher()
   control_msgs::msg::JointTrajectoryControllerState msg_actuators;
 
   msg_actuators = msg_actuators_;
-  for(unsigned int i = 0; i < msg_actuators.joint_names.size(); i++){
-    msg_actuators.joint_names[i] = std::string("motor") + std::to_string(i+1);
-  }
+  for(unsigned int i = 0; i < msg_actuators.joint_names.size(); i++)
+    msg_actuators.joint_names[i] = motor_names[i];
 
   builtin_interfaces::msg::Time stamp = clock_ros.now();
   msg_actuators.header.stamp.sec = stamp.sec;

hros_cognition_mara_components/src/main.cpp

@@ -29,8 +29,7 @@ int main(int argc, char * argv[])
 
   rclcpp::executors::MultiThreadedExecutor exe;
 
-  std::shared_ptr<HROSCognitionMaraComponentsNode> lc_node =
-     std::make_shared<HROSCognitionMaraComponentsNode>(node_name, argc, argv);
+  std::shared_ptr<HROSCognitionMaraComponentsNode> lc_node = std::make_shared<HROSCognitionMaraComponentsNode>(node_name, argc, argv);
 
   exe.add_node(lc_node->get_node_base_interface());