First pass at concept of centralizing motion control in motion control node including docking actions

justinIRBT commented 3 years ago

Description

As we approach implementing the docking action and in the near future, reflexes. We are reaching a point where the velocity commands for the system will need to be centralized and arbitrated. We did something similar in our robot app, so I re-used the paradigm here. What I have here is the starting point for the centralized control.

Motion Control Node subscribes to cmd_vel
Motion Control Node implements dock/undock action API (but contents is unpopulated, @lchico the thought is for your navigation code to go in the action callbacks here)
Motion Control Node subscibes to /dock topic for input to dock actions
Motion Control Node acts as arbiter between cmd_vel and docking actions (and reflexes in near future)
Motion Control Node chooses command and publishes Twist to diffdrive_controller/cmd_vel_unstamped

Fixes # (issue). https://github.com/iRobotEducation/create3_sim/issues/51 https://github.com/iRobotEducation/create3_sim/issues/62

Type of change

[ ] Bug fix (non-breaking change which fixes an issue)
[x] New feature (non-breaking change which adds functionality)

How Has This Been Tested?

Still needs actual docking action velocity commands to be populated by @lchico and /dock topic to be populated by @rjcausarano , but confirmed that in current state publishing to /cmd_vel drives robot in sim after removing republishing node and ros2 action list shows dock and undock

Checklist

I will document code in more detail after we come to consensus that this is the right approach

[x] I have performed a self-review of my own code
[x] I have commented my code, particularly in hard-to-understand areas
[ ] I have made corresponding changes to the documentation

justinIRBT commented 3 years ago

I added a controller based on @lchico approach and integrated it with the docking/undocking actions. I am plugging in a hard coded (0,0,pi) dock position and odometry to the controller. This is fine for undocking, but docking will need a ground truth publication for this approach to work for it. For undocking to work, you have to comment out the check that its not already undocked (as that's not currently updating until https://github.com/iRobotEducation/create3_sim/pull/83 goes in.) Ideally we will have something that reports dock position if the dock moves and something that reports ground truth pose that will plug into the docking behavior to pass to the controller. @rjcausarano Are the values for ground truth dock position and robot position things that are available that could get into the docking behavior through publications?

justinIRBT commented 3 years ago

I added the code to update the dock position from a sub based on the anticipated sim topic. I will still need to change to robot position to update off the sim topic when its available

justinIRBT commented 3 years ago

I have integrated the ground truth pose and my initial test worked with the robot landing on the dock (after commenting out the checks driven by the /dock publication which will be populated on master. I think I have addressed all the comments. @eborghi10 @rjcausarano @alsora, is this good to go? Screenshot from 2021-10-19 07-09-56

rjcausarano commented 3 years ago

I have integrated the ground truth pose and my initial test worked with the robot landing on the dock (after commenting out the checks driven by the /dock publication which will be populated on master. I think I have addressed all the comments. @eborghi10 @rjcausarano @alsora, is this good to go?

What do you mean by commenting out /dock checks? That PR is merged now so if you rabase on top of master you can get that working here

justinIRBT commented 3 years ago

@eborghi10 @rjcausarano This branch has master merged into it, I have verified with ground truth position and dock state feedback, the robot is able to successfully dock/undock through multiple cycles and report the correct state feedback. Assuming the checks pass in the build. Is this good to merge?

justinIRBT commented 3 years ago

@lchico Does this all look good to you

lchico commented 3 years ago

@lchico Does this all look good to you

Sorry for the delay. I will test now!

iRobotEducation / create3_sim