@@ -27,11 +27,23 @@ RobotControlServiceImpl::RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> n
2727 DeclareROSParameter (" servo_in_rate_hz" 10.0 , " [double] Init; Servo commands frequency in Hz."
2828 DeclareROSParameter (" servo_out_rate_hz" 250.0 , " [double] Init; Servo publish loop frequency in Hz."
2929 DeclareROSParameter (" move_group" std::string (" aegis_arm" " [str] Init; Name of the planning group to control."
30+ DeclareROSParameter (
31+ " joint_tolerance" 0.017 ,
32+ " [double] Init; Ignore MoveIt joints call if the joints target is closer than this absolute value in radians."
33+ DeclareROSParameter (
34+ " position_tolerance" 0.001 ,
35+ " [double] Init; Ignore MoveIt pose call if the position target is closer than this absolute value in meters."
36+ DeclareROSParameter (" orientation_tolerance" 0.017 ,
37+ " [double] Init; Ignore MoveIt pose call if the orientation target is closer than this absolute "
38+ " value in radians."
3039
3140 // Runtime parameters
3241 DeclareROSParameter (" r_gripper_close_m" 0.0 , " [double] Runtime; Gripper close position in meters. "
3342 DeclareROSParameter (" r_gripper_open_m" 0.025 , " [double] Runtime; Gripper open position in meters."
3443
44+ JOINT_TOLERANCE_ = node_->get_parameter (" joint_tolerance" as_double ();
45+ POSITION_TOLERANCE_ = node_->get_parameter (" position_tolerance" as_double ();
46+ ORIENTATION_TOLERANCE_ = node_->get_parameter (" orientation_tolerance" as_double ();
3547 servo_tcp_link_ = node_->get_parameter (" servo_link" as_string ();
3648
3749 auto servo_in_hz = node_->get_parameter (" servo_in_rate_hz" as_double ();
@@ -61,6 +73,9 @@ RobotControlServiceImpl::RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> n
6173
6274 auto move_group_name = node_->get_parameter (" move_group" as_string ();
6375 move_group_ = std::make_unique<moveit::planning_interface::MoveGroupInterface>(node_, move_group_name);
76+ move_group_->setPlanningTime (10.0 );
77+ move_group_->setNumPlanningAttempts (10 );
78+ move_group_->startStateMonitor (0.01 );
6479}
6580
6681rclcpp::Logger RobotControlServiceImpl::get_logger () const {
@@ -409,28 +424,43 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context,
409424 return grpc::Status (grpc::StatusCode::INVALID_ARGUMENT, err);
410425 }
411426
412- geometry_msgs::msg::Pose pose;
413- pose.position .x = request->position ().x ();
414- pose.position .y = request->position ().y ();
415- pose.position .z = request->position ().z ();
416- pose.orientation .x = request->orientation ().x ();
417- pose.orientation .y = request->orientation ().y ();
418- pose.orientation .z = request->orientation ().z ();
419- pose.orientation .w = request->orientation ().w ();
427+ auto const target_pose = [&request] {
428+ geometry_msgs::msg::Pose goal;
429+ goal.position .x = request->position ().x ();
430+ goal.position .y = request->position ().y ();
431+ goal.position .z = request->position ().z ();
432+ goal.orientation .x = request->orientation ().x ();
433+ goal.orientation .y = request->orientation ().y ();
434+ goal.orientation .z = request->orientation ().z ();
435+ goal.orientation .w = request->orientation ().w ();
436+ return goal;
437+ }();
438+
439+ if (IsPoseAtGoal (target_pose)) {
440+ RCLCPP_INFO (get_logger (),
441+ " [RobotControlService][GotoPose] Target pose is the same as at the current pose. "
442+ " Skipping planning and returning success."
443+ response->set_msg (" Already at target pose."
444+ return grpc::Status::OK;
445+ }
446+
447+ move_group_->setPoseTarget (target_pose);
420448
421- move_group_->setPoseTarget (pose);
449+ auto const [success, plan] = [&] {
450+ moveit::planning_interface::MoveGroupInterface::Plan msg;
451+ auto const ok = static_cast <bool >(move_group_->plan (msg));
452+ return std::make_pair (ok, msg);
453+ }();
422454
423- moveit::planning_interface::MoveGroupInterface::Plan plan;
424- auto result = move_group_->plan (plan);
425- if (result != moveit::core::MoveItErrorCode::SUCCESS) {
455+ if (!success) {
426456 std::string err = " Planning failed."
427457 RCLCPP_ERROR (get_logger (), " [RobotControlService][GotoPose] %s" c_str ());
428458 response->set_msg (err);
429459 return grpc::Status (grpc::StatusCode::INTERNAL, err);
430460 }
431461
432- auto exec = move_group_->execute (plan);
433- if (exec != moveit::core::MoveItErrorCode::SUCCESS ) {
462+ auto const exec = static_cast < bool >( move_group_->execute (plan) );
463+ if (!exec ) {
434464 std::string err = " Execution failed."
435465 RCLCPP_ERROR (get_logger (), " [RobotControlService][GoToPose] %s" c_str ());
436466 response->set_msg (err);
@@ -442,6 +472,36 @@ grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context,
442472 return grpc::Status::OK;
443473}
444474
475+ bool RobotControlServiceImpl::IsPoseAtGoal (const geometry_msgs::msg::Pose& target_pose) {
476+ geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose ().pose ;
477+
478+ double const position_distance = [&target_pose, ¤t_pose] {
479+ double dx = target_pose.position .x - current_pose.position .x ;
480+ double dy = target_pose.position .y - current_pose.position .y ;
481+ double dz = target_pose.position .z - current_pose.position .z ;
482+ return std::sqrt (dx * dx + dy * dy + dz * dz);
483+ }();
484+
485+ if (position_distance > POSITION_TOLERANCE_)
486+ return false ;
487+
488+ double const angle_distance = [&target_pose, ¤t_pose] {
489+ // Check orientation: convert to roll-pitch-yaw and compare
490+ tf2::Quaternion q1 (target_pose.orientation .x , target_pose.orientation .y , target_pose.orientation .z ,
491+ target_pose.orientation .w );
492+ tf2::Quaternion q2 (current_pose.orientation .x , current_pose.orientation .y , current_pose.orientation .z ,
493+ current_pose.orientation .w );
494+
495+ // Compute angular distance using quaternion dot product
496+ double dot_product = q1.dot (q2);
497+ // Clamp to [-1, 1] to avoid numerical issues with acos
498+ dot_product = std::max (-1.0 , std::min (1.0 , dot_product));
499+ return 2.0 * std::acos (std::abs (dot_product));
500+ }();
501+
502+ return angle_distance < ORIENTATION_TOLERANCE_;
503+ }
504+
445505grpc::Status RobotControlServiceImpl::GotoJoints (grpc::ServerContext* context,
446506 const proto_aegis_grpc::v1::JointState* request,
447507 proto_aegis_grpc::v1::TriggerResponse* response) {
@@ -456,24 +516,40 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context,
456516 return grpc::Status (grpc::StatusCode::INVALID_ARGUMENT, err);
457517 }
458518
459- std::map<std::string, double > target;
460- for (int i = 0 ; i < request->name_size (); ++i) {
461- target[request->name (i)] = request->position (i);
519+ auto const target = [&request] {
520+ std::map<std::string, double > joints_goal;
521+ for (int i = 0 ; i < request->name_size (); ++i) {
522+ joints_goal[request->name (i)] = request->position (i);
523+ }
524+ return joints_goal;
525+ }();
526+
527+ if (AreJointsAtGoal (target)) {
528+ RCLCPP_INFO (get_logger (),
529+ " [RobotControlService][GotoJoints] Already at the target joint configuration. "
530+ " Skipping planning and returning success."
531+ response->set_success (true );
532+ response->set_msg (" Already at target joint positions."
533+ return grpc::Status::OK;
462534 }
463535
464536 move_group_->setJointValueTarget (target);
465537
466- moveit::planning_interface::MoveGroupInterface::Plan plan;
467- auto result = move_group_->plan (plan);
468- if (result != moveit::core::MoveItErrorCode::SUCCESS) {
538+ auto const [success, plan] = [&] {
539+ moveit::planning_interface::MoveGroupInterface::Plan plan;
540+ auto const ok = static_cast <bool >(move_group_->plan (plan));
541+ return std::make_pair (ok, plan);
542+ }();
543+
544+ if (!success) {
469545 std::string err = " Planning failed."
470546 RCLCPP_ERROR (get_logger (), " [RobotControlService][GotoJoints] %s" c_str ());
471547 response->set_msg (err);
472548 return grpc::Status (grpc::StatusCode::INTERNAL, err);
473549 }
474550
475- auto exec = move_group_->execute (plan);
476- if (exec != moveit::core::MoveItErrorCode::SUCCESS ) {
551+ auto const exec = static_cast < bool >( move_group_->execute (plan) );
552+ if (!exec ) {
477553 std::string err = " Execution failed."
478554 RCLCPP_ERROR (get_logger (), " [RobotControlService][GotoJoints] %s" c_str ());
479555 response->set_msg (err);
@@ -485,6 +561,56 @@ grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context,
485561 return grpc::Status::OK;
486562}
487563
564+ bool RobotControlServiceImpl::AreJointsAtGoal (const std::map<std::string, double >& target_joints) {
565+ // Get current state - use timeout for fresh data
566+ moveit::core::RobotStatePtr current_state = move_group_->getCurrentState (0.1 );
567+
568+ // Get joint model group FIRST (needed for copyJointGroupPositions)
569+ const moveit::core::RobotModelConstPtr robot_model = move_group_->getRobotModel ();
570+ const moveit::core::JointModelGroup* joint_model_group = robot_model->getJointModelGroup (move_group_->getName ());
571+
572+ if (!joint_model_group) {
573+ RCLCPP_ERROR (get_logger (), " Failed to get joint model group"
574+ return false ;
575+ }
576+
577+ std::vector<double > current_joints;
578+ current_state->copyJointGroupPositions (joint_model_group, current_joints);
579+
580+ // Check each joint in target
581+ for (const auto & [joint_name, target_value] : target_joints) {
582+ const moveit::core::JointModel* joint_model = joint_model_group->getJointModel (joint_name);
583+
584+ if (!joint_model) {
585+ RCLCPP_WARN (get_logger (), " Joint '%s' not found in planning group" c_str ());
586+ continue ;
587+ }
588+
589+ int joint_index = joint_model_group->getVariableGroupIndex (joint_name);
590+ if (joint_index < 0 || joint_index >= static_cast <int >(current_joints.size ())) {
591+ RCLCPP_WARN (get_logger (), " Joint '%s' invalid index %d (size: %zu)" c_str (), joint_index,
592+ current_joints.size ());
593+ continue ;
594+ }
595+
596+ double current_value = current_joints[joint_index];
597+
598+ if (joint_model->getType () == moveit::core::JointModel::REVOLUTE) {
599+ // For revolute joints, use angular distance (handles wrapping)
600+ double diff = std::abs (target_value - current_value);
601+ double min_angle = std::min (diff, 2.0 * M_PI - diff);
602+ if (min_angle > JOINT_TOLERANCE_)
603+ return false ;
604+ } else {
605+ // Prismatic or other bounded joints - direct comparison
606+ if (std::abs (target_value - current_value) > JOINT_TOLERANCE_)
607+ return false ;
608+ }
609+ }
610+
611+ return true ;
612+ }
613+
488614void RobotControlServiceImpl::GripperSendGoal (double position, double max_effort) {
489615 if (!gripper_client_->wait_for_action_server (action_timeout_)) {
490616 gripper_cmd_success_ = false ;
0 commit comments