Cleanup for cable moderator prototype

[luca-della-vedova]

Targeting iche033/cable_moderator, I tested it and it works quite well, I did some cleaning up, specifically:

• Remove a lot of duplicated variables for 0 and 1 when we can get away with only one (i.e. there is only one detachable joint active, only one set of subscribers, etc.)
• Remove class variables when they can just be local variables
• Refactor duplicated subscription code into a shared function
• Remove outdated commented code
• Add a bunch of TODOs for functions / variables that still exist but seem to be unused, not sure whether they are planned for future use or not but at least it's clear where they are and they can be removed if not needed.

I also removed the spawning of the cable, it turns out it messed up a bit with the namespacing for the pose publisher, we can just spawn the cable at runtime to test this PR.

Test it!

Setup

Run

ros2 launch aic_bringup aic_gz_bringup.launch.py ground_truth:=true spawn_task_board:=true  launch_rviz:=false nic_card_mount_0_present:=true sc_port_0_present:=true spawn_cable:=true attach_cable_to_gripper:=false

This will spawn the simulation with the cable spawned but not attached to the gripper. This plugin will then proceed to attach it.

Manual run, full task

Use gz topic to simulate contacts and manually move the cable

# This will detach the cable, arm will move slightly up, printouts will show that it was detached
gz topic -t /nic_card_mount_0/sfp_port_1/touched --msgtype gz.msgs.Boolean -p "data: True"
# Now use the UI to move the other end of the cable to be within the gripper, you should see that it attaches
# Afterwards, this will attach to the second port and detach from the arm + complete the task
gz topic -t /sc_port_0/sc_port_base/touched --msgtype gz.msgs.Boolean -p "data: True"

End to end (first half of the task) with CheatCode

You can use CheatCode to verify this with an actual task:

ros2 run aic_model aic_model --ros-args -p use_sim_time:=true -p policy:=aic_example_policies.ros.CheatCode
# Another terminal
ros2 lifecycle set /aic_model configure
ros2 lifecycle set /aic_model activate

Copy the following into a script and run it to send the action:

#!/bin/sh
ros2 action send_goal /insert_cable aic_task_interfaces/action/InsertCable "
task:
  id: 'hello'
  cable_type: ''
  cable_name: 'cable_0'
  plug_type: ''
  plug_name: 'sfp_tip'
  port_type: ''
  port_name: 'sfp_port_0'
  target_module_name: 'nic_card_mount_0'
  time_limit: 600"

You should see that when the cable reaches the port the joint is created and the state machine goes to the next state.

[iche033]

awesome! thanks @luca-della-vedova. I went through your instructions and was able to confirm the cable state transitions as well.

I made a few minor cleanup changes in 5c09cde:

• formatting / style
• addressed some TODOs, e.g. removed unused function and variables
• added doc for a couple of new functions
• removed debug statements