refine and clean

DrawZeroPoint · DrawZeroPoint · commit c5c260deb90b · 2022-07-13T17:14:52.000+08:00
diff --git a/include/roport/cartesio_server.h b/include/roport/cartesio_server.h
@@ -66,7 +66,6 @@ class CartesIOServer {
 
   ros::ServiceServer execute_all_poses_srv_;
   ros::ServiceServer execute_all_locked_poses_srv_;
-  ros::ServiceServer execute_mirrored_pose_srv_;
 
   using reachPoseActionClient = actionlib::SimpleActionClient<cartesian_interface::ReachPoseAction>;
   std::vector<std::shared_ptr<reachPoseActionClient>> control_clients_;
@@ -90,11 +89,6 @@ class CartesIOServer {
   auto executeAllLockedPosesSrvCb(roport::ExecuteAllLockedPoses::Request& req,
                                   roport::ExecuteAllLockedPoses::Response& resp) -> bool;
 
-  //  auto executeMirroredPoseSrvCb(roport::ExecuteMirroredPose::Request& req, roport::ExecuteMirroredPose::Response&
-  //  resp)
-  //      -> bool;
-  //
-
   void buildActionGoal(const int& index,
                        const geometry_msgs::Pose& goal_pose,
                        cartesian_interface::ReachPoseActionGoal& action_goal);
@@ -104,14 +98,16 @@ class CartesIOServer {
   auto executeGoals(const std::map<int, cartesian_interface::ReachPoseActionGoal>& goals, double duration = 120)
       -> bool;
 
-  //  static void getMirroredTrajectory(MoveGroupInterface& move_group,
-  //                                    trajectory_msgs::JointTrajectory trajectory,
-  //                                    std::vector<double> mirror_vector,
-  //                                    trajectory_msgs::JointTrajectory& mirrored_trajectory);
-
   bool getTransform(const int& index, geometry_msgs::TransformStamped& transform);
 
-  bool getPose(const int& index, geometry_msgs::Pose& pose);
+  /**
+   * Given the index of the group in group_names_, get current pose of that group's control frame wrt the reference
+   * frame.
+   * @param index Group index in group_names_.
+   * @param pose Pose of the control frame.
+   * @return True if succeed, false otherwise.
+   */
+  bool getCurrentPoseWithIndex(const int& index, geometry_msgs::Pose& pose);
 
   void getGoalPoseWithReference(const int& ref_idx,
                                 const geometry_msgs::Pose& curr_ref_pose,
diff --git a/include/roport/common.h b/include/roport/common.h
@@ -144,6 +144,24 @@ inline void localAlignedPoseToGlobalPose(const geometry_msgs::Pose& pose_local_a
   eigenMatrixToGeometryPose(mat_global_to_target, pose_global_to_target);
 }
 
+inline void toGlobalPose(const int& goal_type,
+                         const geometry_msgs::Pose& current_pose,
+                         const geometry_msgs::Pose& cmd_pose,
+                         geometry_msgs::Pose& goal_pose) {
+  if (goal_type == 0) {
+    // The given pose is already in the reference frame
+    goal_pose = cmd_pose;
+  } else if (goal_type == 1) {
+    // The given pose is relative to the local aligned frame having the same orientation as the reference frame
+    localAlignedPoseToGlobalPose(cmd_pose, current_pose, goal_pose, true);
+  } else if (goal_type == 2) {
+    // The given pose is relative to the local frame
+    localPoseToGlobalPose(cmd_pose, current_pose, goal_pose);
+  } else {
+    throw std::invalid_argument("Goal type not supported");
+  }
+}
+
 inline auto isPoseLegal(const geometry_msgs::Pose& pose) -> bool {
   if (pose.orientation.w == 0 && pose.orientation.x == 0 && pose.orientation.y == 0 && pose.orientation.z == 0) {
     ROS_ERROR("The pose is empty (all orientation coefficients are zero)");
diff --git a/src/lib/cartesio_server.cpp b/src/lib/cartesio_server.cpp
@@ -64,8 +64,6 @@ CartesIOServer::CartesIOServer(const ros::NodeHandle& node_handle, const ros::No
   execute_all_poses_srv_ = nh_.advertiseService("execute_all_poses", &CartesIOServer::executeAllPosesSrvCb, this);
   execute_all_locked_poses_srv_ =
       nh_.advertiseService("execute_all_locked_poses", &CartesIOServer::executeAllLockedPosesSrvCb, this);
-  //  execute_mirrored_pose_srv_ =
-  //      nh_.advertiseService("execute_mirrored_pose", &CartesIOServer::executeMirroredPoseSrvCb, this);
 }
 
 auto CartesIOServer::executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req,
@@ -77,24 +75,12 @@ auto CartesIOServer::executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req,
     for (int j = 0; j < req.group_names.size(); ++j) {
       if (req.group_names[j] == group_names_[i]) {
         geometry_msgs::Pose current_pose;
-        if (!getPose(i, current_pose)) {
+        if (!getCurrentPoseWithIndex(i, current_pose)) {
           return false;
         }
 
-        // Initialize the goal pose
         geometry_msgs::Pose goal_pose;
-
-        // Transfer the given pose to the reference frame
-        if (req.goal_type == req.GLOBAL) {
-          // The given pose is already in the reference frame
-          goal_pose = req.goals.poses[j];
-        } else if (req.goal_type == req.LOCAL_ALIGNED) {
-          // The given pose is relative to the local aligned frame having the same orientation as the reference frame
-          localAlignedPoseToGlobalPose(req.goals.poses[j], current_pose, goal_pose, true);
-        } else {
-          // The given pose is relative to the local frame
-          localPoseToGlobalPose(req.goals.poses[j], current_pose, goal_pose);
-        }
+        toGlobalPose(req.goal_type, current_pose, req.goals.poses[j], goal_pose);
 
         // Build trajectory to reach the goal
         cartesian_interface::ReachPoseActionGoal action_goal;
@@ -130,26 +116,15 @@ auto CartesIOServer::executeAllLockedPosesSrvCb(roport::ExecuteAllLockedPoses::R
 
     geometry_msgs::Pose goal_pose;
     if (j == 0) {
-      if (!getPose(i, current_reference_pose)) {
+      if (!getCurrentPoseWithIndex(i, current_reference_pose)) {
         return false;
       }
       reference_index = i;
-
-      // Transfer the given pose to the reference frame
-      if (req.goal_type == req.GLOBAL) {
-        // The given pose is already in the reference frame
-        goal_pose = req.goal;
-      } else if (req.goal_type == req.LOCAL_ALIGNED) {
-        // The given pose is relative to the local aligned frame having the same orientation as the reference frame
-        localAlignedPoseToGlobalPose(req.goal, current_reference_pose, goal_pose, true);
-      } else {
-        // The given pose is relative to the local frame
-        localPoseToGlobalPose(req.goal, current_reference_pose, goal_pose);
-      }
+      toGlobalPose(req.goal_type, current_reference_pose, req.goal, goal_pose);
       goal_reference_pose = goal_pose;
     } else {
       geometry_msgs::Pose current_pose;
-      if (!getPose(i, current_pose)) {
+      if (!getCurrentPoseWithIndex(i, current_pose)) {
         return false;
       }
       getGoalPoseWithReference(reference_index, current_reference_pose, goal_reference_pose, i, current_pose,
@@ -183,7 +158,7 @@ bool CartesIOServer::getTransform(const int& index, geometry_msgs::TransformStam
   }
 }
 
-bool CartesIOServer::getPose(const int& index, geometry_msgs::Pose& pose) {
+bool CartesIOServer::getCurrentPoseWithIndex(const int& index, geometry_msgs::Pose& pose) {
   geometry_msgs::TransformStamped transform;
   if (!getTransform(index, transform)) {
     return false;
@@ -217,91 +192,6 @@ void CartesIOServer::getGoalPoseWithReference(const int& ref_idx,
   eigenMatrixToGeometryPose(trans_b_g, goal_pose);
 }
 
-// auto CartesIOServer::executeMirroredPoseSrvCb(roport::ExecuteMirroredPose::Request& req,
-//                                               roport::ExecuteMirroredPose::Response& resp) -> bool {
-//   int ref_index = getGroupIndex(req.reference_group);
-//   if (ref_index < 0) {
-//     throw std::runtime_error("Reference group does not exist");
-//   }
-//   int mirror_index = getGroupIndex(req.mirror_group);
-//   if (mirror_index < 0) {
-//     throw std::runtime_error("Mirror group does not exist");
-//   }
-//
-//   std::map<int, trajectory_msgs::JointTrajectory> trajectories;
-//   trajectory_msgs::JointTrajectory trajectory;
-//   geometry_msgs::PoseStamped current_pose_wrt_base = interfaces_[ref_index]->getCurrentPose();
-//   geometry_msgs::PoseStamped goal_pose_wrt_base;
-//   goal_pose_wrt_base.header = current_pose_wrt_base.header;
-//
-//   // Transfer given goal pose to the base frame
-//   if (req.goal_type == req.BASE_ABS) {
-//     goal_pose_wrt_base.pose = req.goal;
-//   } else if (req.goal_type == req.BASE_REL) {
-//     geometry_msgs::PoseStamped transform_wrt_local_base;
-//     transform_wrt_local_base.pose = req.goal;
-//     relativePoseToAbsolutePose(transform_wrt_local_base, current_pose_wrt_base, goal_pose_wrt_base);
-//   } else {
-//     geometry_msgs::PoseStamped goal_pose_wrt_eef;
-//     goal_pose_wrt_eef.pose = req.goal;
-//     goal_pose_wrt_eef.header.frame_id = interfaces_[ref_index]->getEndEffectorLink();
-//     goal_pose_wrt_base =
-//         tf_buffer_.transform(goal_pose_wrt_eef, interfaces_[ref_index]->getPlanningFrame(), ros::Duration(1));
-//   }
-//
-//   // Build trajectory to reach the goal
-//   if (!buildTrajectory(*interfaces_[ref_index], goal_pose_wrt_base, trajectory, req.is_cartesian)) {
-//     ROS_ERROR("RoPort: Building trajectory failed");
-//     resp.result_status = resp.FAILED;
-//     return true;
-//   }
-//
-//   trajectory_msgs::JointTrajectory updated_trajectory;
-//   if (req.stamp > 0) {
-//     updateTrajectoryStamp(trajectory, req.stamp, updated_trajectory);
-//   } else {
-//     updated_trajectory = trajectory;
-//   }
-//   trajectories.insert({ref_index, updated_trajectory});
-//
-//   trajectory_msgs::JointTrajectory mirrored_trajectory;
-//   getMirroredTrajectory(*interfaces_[mirror_index], updated_trajectory, req.mirror_vector, mirrored_trajectory);
-//   trajectories.insert({mirror_index, mirrored_trajectory});
-//
-//   if (executeTrajectories(trajectories)) {
-//     resp.result_status = resp.SUCCEEDED;
-//   } else {
-//     resp.result_status = resp.FAILED;
-//   }
-//   return true;
-// }
-//
-// void CartesIOServer::getMirroredTrajectory(MoveGroupInterface& move_group,
-//                                            trajectory_msgs::JointTrajectory trajectory,
-//                                            std::vector<double> mirror_vector,
-//                                            trajectory_msgs::JointTrajectory& mirrored_trajectory) {
-//   mirrored_trajectory.joint_names = move_group.getActiveJoints();
-//   mirrored_trajectory.header = trajectory.header;
-//   for (size_t i = 0; i < trajectory.points.size(); ++i) {
-//     trajectory_msgs::JointTrajectoryPoint p;
-//     p = trajectory.points[i];
-//     ROS_ASSERT(p.positions.size() == mirror_vector.size());
-//     for (size_t j = 0; j < p.positions.size(); ++j) {
-//       p.positions[j] *= mirror_vector[j];
-//       if (!p.velocities.empty()) {
-//         p.velocities[j] *= mirror_vector[j];
-//       }
-//       if (!p.accelerations.empty()) {
-//         p.accelerations[j] *= mirror_vector[j];
-//       }
-//       if (!p.effort.empty()) {
-//         p.effort[j] *= mirror_vector[j];
-//       }
-//     }
-//     mirrored_trajectory.points.push_back(p);
-//   }
-// }
-
 void CartesIOServer::buildActionGoal(const int& index,
                                      const geometry_msgs::Pose& goal_pose,
                                      cartesian_interface::ReachPoseActionGoal& action_goal) {
diff --git a/srv/ExecuteAllLockedPoses.srv b/srv/ExecuteAllLockedPoses.srv
@@ -1,13 +1,13 @@
-# Move multiple groups to the target poses. One group will be used as the reference, whose goal pose is given,
+# Move multiple groups to the target poses. The first group will be used as the reference, whose goal pose is given,
 # other groups will move according to the reference and kept the relative pose along movement.
 
 # Requests
 # group_names: Names of the moving groups.
-# goal_type: GLOBAL: Absolute pose wrt the base frame (robot base).
-#            LOCAL_ALIGNED: Relative pose wrt the base frame transferred to current eef base position.
+# goal_type: GLOBAL: Absolute pose wrt the reference frame (usually the robot base frame).
+#            LOCAL_ALIGNED: Relative pose wrt the reference frame transferred to current eef base position.
 #            LOCAL: pose wrt the eef frame.
-# goal: Goal pose for the reference group (the first group in the group_names).
-# stamp: Time interval for reaching goals.
+# goal: Goal pose for the reference group (the first group in group_names).
+# stamp: Time interval for all groups reaching goals.
 
 std_msgs/Header header
 string[] group_names
