new yaml structure

README.md

@@ -208,8 +208,6 @@ catkin_make_isolated --install
 ## Gazebo
 Complimentary information is available in our [documentation's simulation section](https://acutronicrobotics.com/docs/technology/h-ros/api/level1/simulation).
 
-Launch MARA!
-
 ```sh
 source ~/ros2_mara_ws/install/setup.bash
 ros2 launch mara_gazebo mara.launch.py
@@ -223,6 +221,8 @@ 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`*
 
+<br/>
+
 ## RViz
 ### Launching MARA's Simulation
 Complimentary information is available in our [documentation's simulation section](https://acutronicrobotics.com/docs/technology/h-ros/api/level1/visualization).
@@ -247,6 +247,8 @@ rviz2 -d `ros2 pkg prefix mara_description`/share/mara_description/rviz/visualiz
 
 Alternatively, instead of using the `robot_description` topic, you can load the 3D model manually selecting the URDF file in RViz.
 
+<br/>
+
 ## MoveIt!
 Motion planning, manipulation, 3D perception, kinematics, control and navigation through brigdes.
 
@@ -255,7 +257,6 @@ Plan trajectories in a virtual environment with Gazebo and MoveIt!.
 
 #### Terminal 1 (ROS 2.0)
 
-Launch MARA:
 ```sh
 source ~/ros2_mara_ws/install/setup.bash
 ros2 launch mara_gazebo mara.launch.py
@@ -289,12 +290,14 @@ roslaunch mara_bringup mara_bringup_moveit_actions.launch gripper:=true prefix:=
 source ~/catkin_mara_ws/devel_isolated/setup.bash
 source ~/ros2_mara_ws/install/setup.bash
 
-ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/individual_trajectories_bridge/config/motors_actions.yaml
+ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/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.
+
 #### Terminal 1 (ROS 2.0)
 
 ```sh
@@ -316,34 +319,30 @@ ros2 launch mara_bringup mara.launch.py --urdf mara_robot_gripper_140
 
 #### Terminal 2 (ROS)
 
-You will need to change the [motors.yaml](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml) file to match the MACs of your SoMs.
-
 ```sh
 source ~/catkin_mara_ws/devel_isolated/setup.bash
-roslaunch mara_bringup mara_bringup_moveit_actions.launch &
+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:
 
 ```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 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 &
 ```
 
 #### Terminal 3 (bridge)
 
-You will need to change the [motors_actions.yaml](https://github.com/AcutronicRobotics/MARA/blob/master/individual_trajectories_bridge/config/motors_actions.yaml) file to match the MACs of your SoMs (MACs should be the same in this yaml file and in the [motors.yaml](https://github.com/AcutronicRobotics/MARA_ROS1/blob/master/mara_bringup/config/motors.yaml) file of the Terminal 2).
-
 ```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
 
-ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/individual_trajectories_bridge/config/motors_actions.yaml &
+ros2 run individual_trajectories_bridge individual_trajectories_bridge_actions -motors ~/ros2_mara_ws/src/mara/hros_cognition_mara_components/config/motors.yaml real &
 ```
 <br/>
 

hros_cognition_mara_components/CMakeLists.txt

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

hros_cognition_mara_components/config/motors.yaml

@@ -0,0 +1,15 @@
+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"
+
+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"

hros_cognition_mara_components/include/hros_cognition_mara_components/HROSCognitionMaraComponents.hpp

@@ -80,6 +80,8 @@ class HROSCognitionMaraComponentsNode : public rclcpp::Node
 	control_msgs::msg::JointTrajectoryControllerState msg_actuators_;
 
   std::string file_motors;
+  std::string environment;
+  std::string motor_key;
 
   std::vector<bool> msg_actuators_callback_sync;
 

hros_cognition_mara_components/src/HROSCognitionMaraComponents.cpp

@@ -42,7 +42,13 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
   }
   std::vector<std::string> lista_subcribers;
 
-  for (auto motor : config["motors"]) {
+  environment = argv[3];
+  if (environment == "sim")
+    motor_key = "simulated_motors";
+  else if (environment == "real")
+    motor_key = "real_motors";
+
+  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;
@@ -53,30 +59,27 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
   for(unsigned int i = 0; i < topic_order.size(); i++){
 
     std::string topic = topic_order[i];
-    std::string delimiter = ":";
+    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;
+    std::string motor_name = id + "state" + axis;
 
     auto subscriber = this->create_subscription<hrim_actuator_rotaryservo_msgs::msg::StateRotaryServo>(
-                              std::string("/") + motor_name,
+                              motor_name,
                               [this, motor_name](hrim_actuator_rotaryservo_msgs::msg::StateRotaryServo::UniquePtr msg) {
                                 stateCallback(motor_name, msg->velocity, msg->position, msg->effort);
                               },rmw_qos_profile_sensor_data);
     motor_state_subcriptions_.push_back(subscriber);
     std::cout << "Subscribe at " << motor_name << std::endl;
 
-    motor_name = id + "/goal_" + axis;
+    motor_name = id + "goal" + axis;
 
-    auto publisher_command = this->create_publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>(
-                                   std::string("/") + motor_name,
-                                   rmw_qos_profile_sensor_data);
+    auto publisher_command = this->create_publisher<hrim_actuator_rotaryservo_msgs::msg::GoalRotaryServo>(motor_name, rmw_qos_profile_sensor_data);
     motor_goal_publishers_.push_back(publisher_command);
     std::cout << "New publisher at " << motor_name << std::endl;
   }
 
   std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
-  std::cout << std::endl << std::endl;
 
   msg_actuators_.actual.positions.resize(motor_goal_publishers_.size());
   msg_actuators_.actual.velocities.resize(motor_goal_publishers_.size());
@@ -86,14 +89,19 @@ HROSCognitionMaraComponentsNode::HROSCognitionMaraComponentsNode(const std::stri
   for(unsigned int i = 0; i < topic_order.size(); i++){
 
     std::string topic = topic_order[i];
-    std::string delimiter = ":";
+    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;
+    std::string motor_name = id + "state" + axis;
 
     msg_actuators_.joint_names[i] =  motor_name;
+
+    std::cout << "motor_name: " << motor_name << std::endl;
   }
 
+  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;

individual_trajectories_bridge/CMakeLists.txt

@@ -73,25 +73,6 @@ include_directories(include
   ${rviz_yaml_cpp_vendor_INCLUDE_DIRS}
 )
 
-add_executable(${PROJECT_NAME} "src/main.cpp")
-
-ament_target_dependencies(${PROJECT_NAME}
-    "rclcpp"
-    "rclcpp_action"
-    "trajectory_msgs"
-    "control_msgs"
-    "yaml_cpp_vendor"
-    ${prefixed_ros1_message_packages}
-    ${ros2_message_packages}
-    "ros1_roscpp"
-		"ros1_control_msgs"
-		"ros1_trajectory_msgs"
-)
-
-target_link_libraries(${PROJECT_NAME}
-  ${YAML_CPP_LIBRARIES}
-)
-
 add_executable(${PROJECT_NAME}_actions
         src/main_actions.cpp
         src/FollowJointTrajectoryAction.cpp)
@@ -116,11 +97,5 @@ target_link_libraries(${PROJECT_NAME}_actions
   ${YAML_CPP_LIBRARIES}
 )
 
-install(
-  PROGRAMS
-  config/motors_actions.yaml
-  DESTINATION share/${PROJECT_NAME}
-)
-
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_actions
+install(TARGETS ${PROJECT_NAME}_actions
   DESTINATION lib/${PROJECT_NAME})