new config file for two robots

README.md

@@ -222,8 +222,6 @@ 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`, `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/>
 
 ## RViz
@@ -263,23 +261,27 @@ If you have used a different urdf in the Terminal 1, you will need to use `urdf:
 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)
+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#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
@@ -327,15 +329,23 @@ roslaunch mara_bringup mara_bringup_moveit_actions.launch env:=real urdf:=mara_r
 
 #### 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

@@ -5,12 +5,6 @@ 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"
@@ -27,9 +21,3 @@ motors:
   - "motor4"
   - "motor5"
   - "motor6"
-  # - "mara2_motor1"
-  # - "mara2_motor2"
-  # - "mara2_motor3"
-  # - "mara2_motor4"
-  # - "mara2_motor5"
-  # - "mara2_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"

mara_bringup/launch/mara.launch.py

@@ -11,10 +11,13 @@ def generate_launch_description():
     Meantime we are going to use sys args.
     '''
     args = sys.argv[1:]
+    configFile = "/share/hros_cognition_mara_components/motors.yaml"
     if "--urdf" in args:
         urdfName = args[args.index("--urdf")+1]
-        if ("train" in urdfName) or ("run" in urdfName):
+        if("train" in urdfName) or ("run" in urdfName):
             urdfName = "reinforcement_learning/" + urdfName
+        if("two" in urdfName):
+            configFile = "/share/hros_cognition_mara_components/two_motors.yaml"
     else:
         urdfName = 'mara_robot'
 
@@ -35,6 +38,6 @@ def generate_launch_description():
 
     ld = LaunchDescription([
         Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf]),
-        Node(package='hros_cognition_mara_components', node_executable='hros_cognition_mara_components', output='screen', arguments=["-motors", install_dir + "/share/hros_cognition_mara_components/motors.yaml", "real"])
+        Node(package='hros_cognition_mara_components', node_executable='hros_cognition_mara_components', output='screen', arguments=["-motors", install_dir + configFile, "real"])
     ])
     return ld

mara_gazebo/launch/mara.launch.py

@@ -9,10 +9,13 @@
 def generate_launch_description():
 
     args = sys.argv[1:]
+    configFile = "/share/hros_cognition_mara_components/motors.yaml"
     if "--urdf" in args:
         urdfName = args[args.index("--urdf")+1]
-        if ("train" in urdfName) or ("run" in urdfName):
+        if("train" in urdfName) or ("run" in urdfName):
             urdfName = "reinforcement_learning/" + urdfName
+        if("two" in urdfName):
+            configFile = "/share/hros_cognition_mara_components/two_motors.yaml"
     else:
         urdfName = 'mara_robot'
 
@@ -48,6 +51,6 @@ def generate_launch_description():
         ),
         Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf]),
         Node(package='mara_utils_scripts', node_executable='spawn_mara.py', arguments=[urdf], output='screen'),
-        Node(package='hros_cognition_mara_components', node_executable='hros_cognition_mara_components', output='screen', arguments=["-motors", install_dir + "/share/hros_cognition_mara_components/motors.yaml", "sim"])
+        Node(package='hros_cognition_mara_components', node_executable='hros_cognition_mara_components', output='screen', arguments=["-motors", install_dir + configFile, "sim"])
     ])
     return ld