diff --git a/src/aruco_pose/include/aruco_pose/aruco_pose.hpp b/src/aruco_pose/include/aruco_pose/aruco_pose.hpp index 34b8382c..47c984df 100644 --- a/src/aruco_pose/include/aruco_pose/aruco_pose.hpp +++ b/src/aruco_pose/include/aruco_pose/aruco_pose.hpp @@ -16,6 +16,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include #include #include +#include #include #include @@ -62,6 +63,12 @@ class ArucoPose : public rclcpp::Node std::vector median_filtered_rpyxyz; + std::unordered_map>> median_filters_map_; + + std::unordered_map> median_filtered_rpyxyz_map_; + + /// @brief converts a rpy to a quaternion geometry_msgs::msg::Quaternion toQuaternion(double roll, double pitch, double yaw); diff --git a/src/aruco_pose/src/aruco_pose.cpp b/src/aruco_pose/src/aruco_pose.cpp index bbd1db47..0ebe3236 100644 --- a/src/aruco_pose/src/aruco_pose.cpp +++ b/src/aruco_pose/src/aruco_pose.cpp @@ -116,7 +116,7 @@ void ArucoPose::image_raw_callback( // Exclude instances where no markers are detected try { - if (!markerIds_.empty() && markerIds_.size() == 1) { + if (!markerIds_.empty()) { // rvecs: rotational vector // tvecs: translation vector std::vector rvecs, tvecs; @@ -126,65 +126,86 @@ void ArucoPose::image_raw_callback( markerCorners_, physical_marker_size_, cameraMatrix_, distCoeffs_, rvecs, tvecs); cv::Mat R; + // Convert the rvecs to a rotation matrix for (size_t i = 0; i < rvecs.size(); ++i) { cv::Rodrigues(rvecs[i], R); - } - // Access each element of the R matrix for easy rpy calculations - double r11 = R.at(0, 0), r21 = R.at(1, 0), r31 = R.at(2, 0), - r32 = R.at(2, 1), r33 = R.at(2, 2); - - double roll, pitch, yaw; - - // rpy calculation - roll = std::atan2(r32, r33); - pitch = std::asin(-r31); - yaw = std::atan2(r21, r11); - - auto tranlsation = tvecs[0]; - - // Construct the raw rpy_xyz of the marker - std::vector raw_rpyxyz = - {roll, pitch, yaw, tranlsation[0], tranlsation[1], tranlsation[2]}; - - // Median filter gets applied - median_filter_->update(raw_rpyxyz, median_filtered_rpyxyz); - - // Add to the tf frame here for the sample - geometry_msgs::msg::TransformStamped transformStamped_tag; - - transformStamped_tag.header.stamp = this->now(); - transformStamped_tag.header.frame_id = this->get_parameter("camera_tf_frame").as_string(); - transformStamped_tag.child_frame_id = this->get_parameter("sample_name").as_string(); - - transformStamped_tag.transform.translation.x = median_filtered_rpyxyz[3] + - this->get_parameter("offset_on_marker_x").as_double(); - transformStamped_tag.transform.translation.y = median_filtered_rpyxyz[4] + - this->get_parameter("offset_on_marker_y").as_double(); - transformStamped_tag.transform.translation.z = median_filtered_rpyxyz[5]; - transformStamped_tag.transform.rotation = toQuaternion( - median_filtered_rpyxyz[0], - median_filtered_rpyxyz[1], - median_filtered_rpyxyz[2]); - - // Add a pre-pickup tf - geometry_msgs::msg::TransformStamped transformStamped_pre_pickup; - transformStamped_pre_pickup.header.stamp = this->now(); - transformStamped_pre_pickup.header.frame_id = this->get_parameter("sample_name").as_string(); - transformStamped_pre_pickup.child_frame_id = - this->get_parameter("pre_pickup_location.name").as_string(); - - transformStamped_pre_pickup.transform.translation.x = this->get_parameter( - "pre_pickup_location.x_adj").as_double(); - transformStamped_pre_pickup.transform.translation.y = this->get_parameter( - "pre_pickup_location.y_adj").as_double(); - transformStamped_pre_pickup.transform.translation.z = this->get_parameter( - "pre_pickup_location.z_adj").as_double(); - transformStamped_pre_pickup.transform.rotation = toQuaternion(0, 0, 0); - - static_broadcaster_.sendTransform(transformStamped_pre_pickup); - static_broadcaster_.sendTransform(transformStamped_tag); + int id = markerIds_[i]; + + if (median_filters_map_.find(id) == median_filters_map_.end()) { + RCLCPP_INFO(this->LOGGER, "New ID found : %d ", id); + median_filters_map_.insert_or_assign( + id, + std::make_shared>()); + + std::vector vec = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + median_filtered_rpyxyz_map_.insert_or_assign(id, vec); + } + + // Access each element of the R matrix for easy rpy calculations + double r11 = R.at(0, 0), r21 = R.at(1, 0), r31 = R.at(2, 0), + r32 = R.at(2, 1), r33 = R.at(2, 2); + + double roll, pitch, yaw; + + // rpy calculation + roll = std::atan2(r32, r33); + pitch = std::asin(-r31); + yaw = std::atan2(r21, r11); + + auto tranlsation = tvecs[i]; + // RCLCPP_INFO(this->LOGGER, "ID : %d, tranlsation[0] : %f ", id, tranlsation[0]); + + // Construct the raw rpy_xyz of the marker + std::vector raw_rpyxyz = + {roll, pitch, yaw, tranlsation[0], tranlsation[1], tranlsation[2]}; + + // Median filter gets applied + // median_filter_->update(raw_rpyxyz, median_filtered_rpyxyz); + + median_filtered_rpyxyz = raw_rpyxyz; + // median_filters_map_[id]->update(raw_rpyxyz, median_filtered_rpyxyz); + + // RCLCPP_INFO( + // this->LOGGER, "ID : %d, median_filtered_rpyxyz_map_[id] : %f ", id, + // median_filtered_rpyxyz_map_[id][0]); + + // Add to the tf frame here for the sample + geometry_msgs::msg::TransformStamped transformStamped_tag; + + transformStamped_tag.header.stamp = this->now(); + transformStamped_tag.header.frame_id = this->get_parameter("camera_tf_frame").as_string(); + transformStamped_tag.child_frame_id = std::to_string(id); + + transformStamped_tag.transform.translation.x = median_filtered_rpyxyz[3] + + this->get_parameter("offset_on_marker_x").as_double(); + transformStamped_tag.transform.translation.y = median_filtered_rpyxyz[4] + + this->get_parameter("offset_on_marker_y").as_double(); + transformStamped_tag.transform.translation.z = median_filtered_rpyxyz[5]; + transformStamped_tag.transform.rotation = toQuaternion( + median_filtered_rpyxyz[0], + median_filtered_rpyxyz[1], + median_filtered_rpyxyz[2]); + + // Add a pre-pickup tf + geometry_msgs::msg::TransformStamped transformStamped_pre_pickup; + transformStamped_pre_pickup.header.stamp = this->now(); + transformStamped_pre_pickup.header.frame_id = std::to_string(id); + transformStamped_pre_pickup.child_frame_id = + std::to_string(id) + "_" + this->get_parameter("pre_pickup_location.name").as_string(); + + transformStamped_pre_pickup.transform.translation.x = this->get_parameter( + "pre_pickup_location.x_adj").as_double(); + transformStamped_pre_pickup.transform.translation.y = this->get_parameter( + "pre_pickup_location.y_adj").as_double(); + transformStamped_pre_pickup.transform.translation.z = this->get_parameter( + "pre_pickup_location.z_adj").as_double(); + transformStamped_pre_pickup.transform.rotation = toQuaternion(0, 0, 0); + + static_broadcaster_.sendTransform(transformStamped_pre_pickup); + static_broadcaster_.sendTransform(transformStamped_tag); + } } // Inner try-catch diff --git a/src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp b/src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp index 6a8056ea..3684c0a0 100644 --- a/src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp +++ b/src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp @@ -5,6 +5,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/ #include #include #include +#include #include #include @@ -57,4 +58,8 @@ class PdfBeamtimeFidPoseServer : public PdfBeamtimeServer bool pickup_pose_saved = false; std::vector pre_pickup_approach_joints_; std::vector pickup_joints_; + int sample_id = 0; + + std::unordered_map> pickup_storage_map_; + std::unordered_map> pre_pickup_approach_storage_map_; }; diff --git a/src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp b/src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp index 0a7cd540..3f121193 100644 --- a/src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp +++ b/src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp @@ -29,8 +29,8 @@ class TFUtilities std::shared_ptr parameters_client_; - std::string world_frame, sample_frame, grasping_point_on_gripper_frame, wrist_2_frame, - pre_pickup_approach_point_frame; + std::string world_frame, grasping_point_on_gripper_frame, wrist_2_frame, + pre_pickup_approach_point_frame_suffix_; public: explicit TFUtilities(const rclcpp::Node::SharedPtr node); @@ -39,14 +39,14 @@ class TFUtilities double degreesToRadians(double degrees); std::pair get_wrist_elbow_alignment( - moveit::planning_interface::MoveGroupInterface & mgi); + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id); std::vector get_pickup_action_z_adj( - moveit::planning_interface::MoveGroupInterface & mgi); + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id); std::vector get_pickup_action_pre_pickup( - moveit::planning_interface::MoveGroupInterface & mgi); + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id); std::vector get_pickup_action_pickup( - moveit::planning_interface::MoveGroupInterface & mgi); + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id); }; diff --git a/src/pdf_beamtime/src/pdf_beamtime_fidpose_client.py b/src/pdf_beamtime/src/pdf_beamtime_fidpose_client.py index 7cfdae94..7c772195 100644 --- a/src/pdf_beamtime/src/pdf_beamtime_fidpose_client.py +++ b/src/pdf_beamtime/src/pdf_beamtime_fidpose_client.py @@ -23,13 +23,12 @@ def send_pickup_goal(self): """Send a working goal.""" goal_msg = FidPoseControlMsg.Goal() - goal_msg.inbeam_approach = [55.10, -51.78, 124.84, -73.16, 52.24, 180.0] - goal_msg.inbeam_approach = [x / 180 * math.pi for x in goal_msg.inbeam_approach] + goal_msg.inbeam_approach = [x / 180 * math.pi for x in [55.10, -51.78, 124.84, -73.16, 52.24, 180.0]] - goal_msg.inbeam = [63.84, -43.13, 98.29, -55.25, 61.00, 180.0] - goal_msg.inbeam = [x / 180 * math.pi for x in goal_msg.inbeam] + goal_msg.inbeam = [x / 180 * math.pi for x in [63.84, -43.13, 98.29, -55.25, 61.00, 180.0]] goal_msg.sample_return = False + goal_msg.sample_id = 150 self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) @@ -45,6 +44,7 @@ def send_retrun_sample_goal(self): goal_msg.inbeam = [x / 180 * math.pi for x in goal_msg.inbeam] goal_msg.sample_return = True + goal_msg.sample_id = 150 self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) @@ -67,8 +67,8 @@ def main(args=None): rclpy.init(args=args) client = SimpleClient() - client.send_pickup_goal() - # client.send_retrun_sample_goal() + # client.send_pickup_goal() + client.send_retrun_sample_goal() rclpy.spin(client) diff --git a/src/pdf_beamtime/src/pdf_beamtime_fidpose_server.cpp b/src/pdf_beamtime/src/pdf_beamtime_fidpose_server.cpp index 5d86c662..0a99dac4 100644 --- a/src/pdf_beamtime/src/pdf_beamtime_fidpose_server.cpp +++ b/src/pdf_beamtime/src/pdf_beamtime_fidpose_server.cpp @@ -25,10 +25,14 @@ rclcpp_action::GoalResponse PdfBeamtimeFidPoseServer::fidpose_handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { + RCLCPP_INFO(node_->get_logger(), "Goal handle inside"); (void)uuid; if (goal->sample_return && !pickup_pose_saved) { + RCLCPP_INFO(node_->get_logger(), "Goal reject"); return rclcpp_action::GoalResponse::REJECT; } else { + RCLCPP_INFO(node_->get_logger(), "Goal accept and execute"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } } @@ -88,6 +92,13 @@ void PdfBeamtimeFidPoseServer::execute( // Decide if it'a a return ot a place if (fidpose_goal->sample_return) { + // Abort goal is asked to return to an unsaved storage location + if (pickup_storage_map_.find(fidpose_goal->sample_id) == pickup_storage_map_.end()) { + RCLCPP_ERROR(node_->get_logger(), "Unrecorded return location at storage"); + results->success = false; + goal_handle->abort(results); + return; + } fsm_results = run_return_fsm(fidpose_goal); } else { fsm_results = run_fsm(fidpose_goal); @@ -119,7 +130,6 @@ void PdfBeamtimeFidPoseServer::execute( set_current_state(State::HOME); results->success = true; goal_handle->succeed(results); - pickup_pose_saved = true; return; } } @@ -133,6 +143,8 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( node_->get_logger(), "Executing state %s", external_state_names_[static_cast(current_state_)].c_str()); moveit::core::MoveItErrorCode motion_results = moveit::core::MoveItErrorCode::FAILURE; + + sample_id = goal->sample_id; switch (current_state_) { case State::HOME: // Moves the robot to pickup approach. @@ -153,8 +165,9 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( case State::PICKUP_APPROACH: { // Moves the robot to pickup in two steps // 1. Adust the wrist 3 and wrist 2 positions to face the gripper towards the sample + std::pair new_wrist_angles = tf_utilities_->get_wrist_elbow_alignment( - move_group_interface_); + move_group_interface_, sample_id); adjusted_pickup_ = pickup_approach_; adjusted_pickup_[4] = new_wrist_angles.first; @@ -166,14 +179,14 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( // 2.1 Adjust the z distance motion_results = inner_state_machine_->move_robot_cartesian( move_group_interface_, tf_utilities_->get_pickup_action_z_adj( - move_group_interface_)); + move_group_interface_, sample_id)); if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} inner_state_machine_->set_internal_state(Internal_State::RESTING); // 2.2 Cartesian move to pre-pickup location in front of the sample motion_results = inner_state_machine_->move_robot_cartesian( move_group_interface_, tf_utilities_->get_pickup_action_pre_pickup( - move_group_interface_)); + move_group_interface_, sample_id)); if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} inner_state_machine_->set_internal_state(Internal_State::RESTING); @@ -186,7 +199,7 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( // 2.3 Cartesian move to pickup motion_results = inner_state_machine_->move_robot_cartesian( move_group_interface_, tf_utilities_->get_pickup_action_pickup( - move_group_interface_)); + move_group_interface_, sample_id)); if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} @@ -196,6 +209,9 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( "ur_arm"), pickup_joints_); + // Insert the saved joint positions to maps + pickup_storage_map_.insert_or_assign(sample_id, pickup_joints_); + pre_pickup_approach_storage_map_.insert_or_assign(sample_id, pre_pickup_approach_joints_); // State propagation inner_state_machine_->set_internal_state(Internal_State::RESTING); set_current_state(State::PICKUP); @@ -220,7 +236,6 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( break; case State::GRASP_SUCCESS: - // if (!goal->sample_return) { // Successfully grasped. Do pickup retreat motion_results = inner_state_machine_->move_robot( move_group_interface_, pre_pickup_approach_joints_); @@ -320,7 +335,7 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( set_current_state(State::HOME); inner_state_machine_->set_internal_state(Internal_State::RESTING); progress_ = progress_ + 1.0; - pickup_pose_saved = false; + pickup_pose_saved = true; break; default: @@ -333,6 +348,7 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_fsm( moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_return_fsm( std::shared_ptr goal) { + sample_id = goal->sample_id; RCLCPP_INFO( node_->get_logger(), "Executing state %s", external_state_names_[static_cast(current_state_)].c_str()); @@ -384,7 +400,6 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_return_fsm( break; case State::GRASP_SUCCESS: - // if (!goal->sample_return) { // Successfully grasped. Do pickup retreat to inbeam approach motion_results = inner_state_machine_->move_robot( move_group_interface_, goal->inbeam_approach); @@ -426,13 +441,14 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_return_fsm( motion_results = inner_state_machine_->move_robot( move_group_interface_, - pre_pickup_approach_joints_); + pre_pickup_approach_storage_map_[sample_id]); if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} inner_state_machine_->set_internal_state(Internal_State::RESTING); motion_results = inner_state_machine_->move_robot( move_group_interface_, - pickup_joints_); + pickup_storage_map_[sample_id]); + if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} inner_state_machine_->set_internal_state(Internal_State::RESTING); @@ -458,10 +474,9 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_return_fsm( case State::RELEASE_SUCCESS: // Sample was successfully released. - // if (!goal->sample_return) { motion_results = inner_state_machine_->move_robot( move_group_interface_, - pre_pickup_approach_joints_); + pre_pickup_approach_storage_map_[sample_id]); if (motion_results == moveit::core::MoveItErrorCode::FAILURE) {break;} inner_state_machine_->set_internal_state(Internal_State::RESTING); @@ -485,6 +500,7 @@ moveit::core::MoveItErrorCode PdfBeamtimeFidPoseServer::run_return_fsm( set_current_state(State::HOME); inner_state_machine_->set_internal_state(Internal_State::RESTING); progress_ = progress_ + 1.0; + pickup_pose_saved = false; break; default: @@ -547,7 +563,7 @@ void PdfBeamtimeFidPoseServer::execute_cleanup() case State::GRASP_FAILURE: { // Move back to pickup approach std::pair new_wrist_angles = tf_utilities_->get_wrist_elbow_alignment( - move_group_interface_); + move_group_interface_, sample_id); adjusted_pickup_ = goal->pickup_approach; adjusted_pickup_[4] = new_wrist_angles.first; adjusted_pickup_[5] = new_wrist_angles.second; @@ -560,7 +576,7 @@ void PdfBeamtimeFidPoseServer::execute_cleanup() case State::RELEASE_FAILURE: case State::RELEASE_SUCCESS: { std::pair new_wrist_angles = tf_utilities_->get_wrist_elbow_alignment( - move_group_interface_); + move_group_interface_, sample_id); adjusted_place_ = goal->place_approach; adjusted_place_[4] = new_wrist_angles.first; adjusted_place_[5] = new_wrist_angles.second; diff --git a/src/pdf_beamtime/src/tf_utilities.cpp b/src/pdf_beamtime/src/tf_utilities.cpp index 9012f770..530901e9 100644 --- a/src/pdf_beamtime/src/tf_utilities.cpp +++ b/src/pdf_beamtime/src/tf_utilities.cpp @@ -19,8 +19,7 @@ TFUtilities::TFUtilities(const rclcpp::Node::SharedPtr node) RCLCPP_INFO(tf_util_logger_, "Waiting for the parameters service to be available..."); } - sample_frame = parameters_client_->get_parameter("sample_name"); - pre_pickup_approach_point_frame = parameters_client_->get_parameter( + pre_pickup_approach_point_frame_suffix_ = parameters_client_->get_parameter( "pre_pickup_location.name"); world_frame = "world"; @@ -35,11 +34,15 @@ double TFUtilities::degreesToRadians(double degrees) } std::pair TFUtilities::get_wrist_elbow_alignment( - moveit::planning_interface::MoveGroupInterface & mgi) + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id) { tf2::Quaternion tf2_wrist_2_quaternion; tf2::Quaternion tf2_sample_quaternion; + std::string sample_frame = std::to_string(sample_id); + std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" + + pre_pickup_approach_point_frame_suffix_; + geometry_msgs::msg::TransformStamped transform_world_to_sample; geometry_msgs::msg::TransformStamped transform_world_to_wrist_2; @@ -78,13 +81,16 @@ std::pair TFUtilities::get_wrist_elbow_alignment( } std::vector TFUtilities::get_pickup_action_z_adj( - moveit::planning_interface::MoveGroupInterface & mgi) + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id) { // Define waypoints for Cartesian path std::vector waypoints; geometry_msgs::msg::TransformStamped transform_world_to_grasping_point; geometry_msgs::msg::TransformStamped transform_world_to_pickup_approach_point; + std::string sample_frame = std::to_string(sample_id); + std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" + + pre_pickup_approach_point_frame_suffix_; double z_dist_to_pickup_approach = 0.0; while (rclcpp::ok()) { @@ -123,13 +129,17 @@ std::vector TFUtilities::get_pickup_action_z_adj( } std::vector TFUtilities::get_pickup_action_pre_pickup( - moveit::planning_interface::MoveGroupInterface & mgi) + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id) { // Define waypoints for Cartesian path std::vector waypoints; geometry_msgs::msg::TransformStamped transform_world_to_grasping_point; geometry_msgs::msg::TransformStamped transform_world_to_pickup_approach_point; + std::string sample_frame = std::to_string(sample_id); + std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" + + pre_pickup_approach_point_frame_suffix_; + double x_dist_to_pickup_approach = 0.0, y_dist_to_pickup_approach = 0.0; while (rclcpp::ok()) { @@ -180,13 +190,17 @@ std::vector TFUtilities::get_pickup_action_pre_pickup( } std::vector TFUtilities::get_pickup_action_pickup( - moveit::planning_interface::MoveGroupInterface & mgi) + moveit::planning_interface::MoveGroupInterface & mgi, int sample_id) { // Define waypoints for Cartesian path std::vector waypoints; geometry_msgs::msg::TransformStamped transform_world_to_sample; geometry_msgs::msg::TransformStamped transform_world_to_pickup_approach_point; + std::string sample_frame = std::to_string(sample_id); + std::string pre_pickup_approach_point_frame = std::to_string(sample_id) + "_" + + pre_pickup_approach_point_frame_suffix_; + double x_dist_to_sample = 0.0, y_dist_to_sample = 0.0; while (rclcpp::ok()) { diff --git a/src/pdf_beamtime_interfaces/action/FidPoseControlMsg.action b/src/pdf_beamtime_interfaces/action/FidPoseControlMsg.action index 112511b2..2db3901c 100644 --- a/src/pdf_beamtime_interfaces/action/FidPoseControlMsg.action +++ b/src/pdf_beamtime_interfaces/action/FidPoseControlMsg.action @@ -1,6 +1,7 @@ float64[] inbeam_approach float64[] inbeam bool sample_return +int32 sample_id --- bool success ---