Update ros2 grpah

docs/software_design.md

@@ -1,45 +1,96 @@
 # Software Design
 
 ## Description
-The software stack of the ASL Trunk Robot is primarily developed in ROS2, and can be divided into three components: main, motion capture and motor control.
+The software stack of the ASL Trunk Robot is primarily developed in ROS2, and can is divided into the components:
+*main*, *motion capture*, *motor control*, *camera* and *gripper*.
+
 We assume that several computing resources are available: a Rapsberry Pi for executing the motor control, a main linux machine for running the experiments, and a Windows machine for running the motion capture Motive software.
 
 ## ROS2 Graph
-The diagrams are generated using [ros2_graph](https://github.com/kiwicampus/ros2_graph/).
+We provide an overall architecture of the ROS2 nodes, topics and services used in the software stack.
+This diagram is insipred by [ros2_graph](https://github.com/kiwicampus/ros2_graph/).
 
 ```mermaid
-flowchart LR
-
-/data_collection_node[ /data_collection_node ]:::main
-/converter_node[ /converter_node ]:::node
-/trunk_rigid_bodies([ /trunk_rigid_bodies<br>interfaces/msg/TrunkRigidBodies ]):::topic
-/all_motors_control([ /all_motors_control<br>interfaces/msg/AllMotorsControl ]):::topic
-
-
-/trunk_rigid_bodies --> /data_collection_node
-/all_motors_control --> /converter_node
-/data_collection_node --> /all_motors_control
-/converter_node --> /trunk_rigid_bodies
-
-
-
+flowchart 
+
+/run_experiment_node[ /run_experiment_node ]:::main
+
+/converter_node_motors[ /converter_node motors ]:::node
+/converter_node_mocap[ /converter_node mocap ]:::node
+/ros_phoenix_node[ /ros_phoenix_node ]:::node
+%% /mocap4ros2_optitrack_node[ /mocap4ros2_optitrack_node ]:::node
+%% /v4l2_camera_node[ /v4l2_camera_node ]:::node
+/servo_control_node[ /servo_control_node ]:::node
+/mpc_solver_node[ /mpc_solver_node ]:::node
+
+/rigid_bodies([ /rigid_bodies ]):::topic
+/markers([ /markers ]):::topic
+/trunk_rigid_bodies([ /trunk_rigid_bodies ]):::topic
+/trunk_markers([ /trunk_markers ]):::topic
+/image_raw([ /image_raw ]):::topic
+/talon1_set([ /talon1/set ]):::topic
+/talon2_set([ /talon2/set ]):::topic
+/talon3_set([ /talon3/set ]):::topic
+/talon4_set([ /talon4/set ]):::topic
+/talon5_set([ /talon5/set ]):::topic
+/talon6_set([ /talon6/set ]):::topic
+%% /talon1_status([ /talon1/status ]):::topic
+%% /talon2_status([ /talon2/status ]):::topic
+%% /talon3_status([ /talon3/status ]):::topic
+%% /talon4_status([ /talon4/status ]):::topic
+%% /talon5_status([ /talon5/status ]):::topic
+%% /talon6_status([ /talon6/status ]):::topic
+/all_motors_status([ /all_motors_status ]):::topic
+/all_motors_control([ /all_motors_control ]):::topic
+
+/move_gripper[/move_gripper\]:::service
+/mpc_solver[/mpc_solver\]:::service
+/ik_solver[/ik_solver\]:::service
+
+/run_experiment_node --> /all_motors_control --> /converter_node_motors
+/converter_node_motors --> /talon1_set --> /ros_phoenix_node
+/converter_node_motors --> /talon2_set --> /ros_phoenix_node
+/converter_node_motors --> /talon3_set --> /ros_phoenix_node
+/converter_node_motors --> /talon4_set --> /ros_phoenix_node
+/converter_node_motors --> /talon5_set --> /ros_phoenix_node
+/converter_node_motors --> /talon6_set --> /ros_phoenix_node
+%% /ros_phoenix_node --> /talon1_status --> /converter_node_motors
+%% /ros_phoenix_node --> /talon2_status --> /converter_node_motors
+%% /ros_phoenix_node --> /talon3_status --> /converter_node_motors
+%% /ros_phoenix_node --> /talon4_status --> /converter_node_motors
+%% /ros_phoenix_node --> /talon5_status --> /converter_node_motors
+%% /ros_phoenix_node --> /talon6_status --> /converter_node_motors
+/converter_node_motors --> /all_motors_status --> /run_experiment_node
+
+/rigid_bodies --> /converter_node_mocap
+/markers --> /converter_node_mocap
+/converter_node_mocap --> /trunk_rigid_bodies --> /run_experiment_node
+/converter_node_mocap --> /trunk_markers --> /run_experiment_node
+
+%% /v4l2_camera_node --> /image_raw --> /run_experiment_node
+/image_raw --> /run_experiment_node
+
+/run_experiment_node <==> /move_gripper o==o /servo_control_node
+/run_experiment_node <==> /mpc_solver o==o /mpc_solver_node 
+/run_experiment_node <==> /ik_solver o==o /ik_solver_node 
 
 subgraph keys[<b>Keys<b/>]
 subgraph nodes[<b><b/>]
-topicb((No connected)):::bugged
+topicb((Not connected)):::bugged
 main_node[main]:::main
 end
 subgraph connection[<b><b/>]
 node1[node1]:::node
 node2[node2]:::node
-node1 o-.-o|to server| service[/Service<br>service/Type\]:::service
+node1 o-.-o|to server| service[/Service\]:::service
 service <-.->|to client| node2
-node1 -->|publish| topic([Topic<br>topic/Type]):::topic
+node1 -->|publish| topic([Topic]):::topic
 topic -->|subscribe| node2
-node1 o==o|to server| action{{/Action<br>action/Type/}}:::action
+node1 o==o|to server| action{{Action}}:::action
 action <==>|to client| node2
 end
 end
+
 classDef node opacity:0.9,fill:#2A0,stroke:#391,stroke-width:4px,color:#fff
 classDef action opacity:0.9,fill:#66A,stroke:#225,stroke-width:2px,color:#fff
 classDef service opacity:0.9,fill:#3B8062,stroke:#3B6062,stroke-width:2px,color:#fff

stack/mocap/src/converter/converter/converter_node.py

@@ -37,7 +37,6 @@ def marker_callback(self, msg):
         trunk_msg.frame_number = msg.frame_number
         trunk_msg.translations = [marker.translation for marker in msg.markers]
         self.publisher.publish(trunk_msg)
-        # self.get_logger().info('Published TrunkMarkers message.')
 
     def rigid_body_callback(self, msg):
         trunk_msg = TrunkRigidBodies()
@@ -46,7 +45,6 @@ def rigid_body_callback(self, msg):
         trunk_msg.rigid_body_names = [rigid_body.rigid_body_name for rigid_body in msg.rigidbodies]
         trunk_msg.positions = [rigid_body.pose.position for rigid_body in msg.rigidbodies]
         self.publisher.publish(trunk_msg)
-        # self.get_logger().info('Published TrunkRigidBodies message.')
 
 
 def main(args=None):