Humble fiducial pose integration into pdf_beamtime #42
Conversation
ChandimaFernando
changed the title
There was a problem hiding this comment.
It's looking great. I think a little bit of clarity, and this is good to go.
For next steps, we should:
- Get some documentation attached to the repo with diagrams, especially of all of the FSMs.
- Think about potential refactors.
- A 3rd action that considers multiple sample holders. It will only need to take the sample uid or number, with the in-beam positions in the parameter server. I'm content with them being redundantly passed as messages now, it seems better for prototyping.
| rclcpp_action::GoalResponse PdfBeamtimeFidPoseServer::fidpose_handle_goal( | ||
| const rclcpp_action::GoalUUID & uuid, | ||
| std::shared_ptr<const FidPoseControlMsg::Goal> goal) | ||
| { | ||
| (void)uuid; | ||
| return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; | ||
| } |
There was a problem hiding this comment.
This is where I would expect some logic on whether or not we have an existing position to return to?
There was a problem hiding this comment.
Logic added to reject if a return goal is issued before return positions are saved
erobs/src/pdf_beamtime/src/pdf_beamtime_fidpose_server.cpp
Lines 29 to 33 in a54c588
| if (fidpose_goal->sample_return) { | ||
| if (pickup_pose_saved) { | ||
| fsm_results = run_return_fsm(fidpose_goal); | ||
| } else { | ||
| RCLCPP_ERROR(node_->get_logger(), "Pickup poses are not set!"); | ||
| return; | ||
| } |
There was a problem hiding this comment.
It's fine to leave this error logic as a failsafe, but shouldn't the goal itself should be rejected if its not a feasible goal?
There was a problem hiding this comment.
Resolved in the earlier comment.
| int main(int argc, char * argv[]) | ||
| { | ||
| rclcpp::init(argc, argv); | ||
|
|
||
| // Create a ROS logger for main scope | ||
| auto const logger = rclcpp::get_logger("hello_moveit"); | ||
| using namespace std::chrono_literals; | ||
|
|
||
| // Create a node for synchronously grabbing params | ||
| auto parameter_client_node = rclcpp::Node::make_shared("param_client"); | ||
| auto parent_parameters_client = | ||
| std::make_shared<rclcpp::SyncParametersClient>(parameter_client_node, "move_group"); | ||
| // Boiler plate wait block | ||
| while (!parent_parameters_client->wait_for_service(1s)) { | ||
| if (!rclcpp::ok()) { | ||
| RCLCPP_ERROR( | ||
| logger, "Interrupted while waiting for the service. Exiting."); | ||
| return 0; | ||
| } | ||
| RCLCPP_INFO(logger, "move_group service not available, waiting again..."); | ||
| } | ||
| // Get robot config parameters from parameter server | ||
| auto parameters = parent_parameters_client->get_parameters( | ||
| {"robot_description_semantic", | ||
| "robot_description"}); | ||
|
|
||
| // Set node parameters using NodeOptions | ||
| rclcpp::NodeOptions node_options; | ||
| node_options.automatically_declare_parameters_from_overrides(true); | ||
| node_options.parameter_overrides( | ||
| { | ||
| {"robot_description_semantic", parameters[0].value_to_string()}, | ||
| {"robot_description", parameters[1].value_to_string()} | ||
| }); | ||
|
|
||
| rclcpp::executors::MultiThreadedExecutor executor; | ||
|
|
||
| auto beamtime_server = std::make_shared<PdfBeamtimeFidPoseServer>( | ||
| "ur_arm", | ||
| node_options); | ||
|
|
||
| executor.add_node(beamtime_server->getNodeBaseInterface()); | ||
| executor.add_node(beamtime_server->getInterruptNodeBaseInterface()); | ||
| executor.spin(); | ||
|
|
||
| rclcpp::shutdown(); | ||
| return 0; | ||
| } |
There was a problem hiding this comment.
Why an entry point for the original server, but not an entry point here? Happy for either, but consistency would help.
There was a problem hiding this comment.
| moveit::core::MoveItErrorCode run_fsm( | ||
| std::shared_ptr<const pdf_beamtime_interfaces::action::FidPoseControlMsg_Goal> goal); | ||
|
|
||
| moveit::core::MoveItErrorCode run_return_fsm( | ||
| std::shared_ptr<const pdf_beamtime_interfaces::action::FidPoseControlMsg_Goal> goal); |
There was a problem hiding this comment.
Do we need a different FSM for each?
There was a problem hiding this comment.
Both are the same fsm but function differently in sample place vs return.
Decided to re-structure if needed on a later PR.
This PR creates two modes of pdf_beamtime sample manipulations with two ROS executables.
Node: pdf_beamtime_server
It takes four joint space coordinates [pickup approach, pickup, place approach, and place] for picking a sample from a predefined storage and placing it at predefined in-beam location.
Run
ros2 launch pdf_beamtime pdf_beamtime.launch.pyto launch pdf_beamtime_server.Run
python3 src/pdf_beamtime/src/pdf_beamtime_client.pyto send action client for sample manipulation.pdf_beamtime_fidpose_server
This takes three joint space coordinates and a boolean flag to indicate if it's for picking up the sample [inbeam_approach, inbeam, sample_return] and uses the pose generated through aruco_pose node to estimate the pose of the sample holder. Expected behavior is below:
On return, it takes three joint space coordinates and a boolean flag to indicate if it's to return the sample [inbeam_approach, inbeam, sample_return] and uses the pose estimated in picking up the sample to place the sample at the storage. Expected behavior is below:
Run
ros2 launch pdf_beamtime pdf_beamtime_fidpose_server.launch.pyto launch pdf_beamtime_fidpose_server.Run
python3 src/pdf_beamtime/src/pdf_beamtime_fidpose_client.pyto send action client for sample manipulation.