Merge pull request #4 from DrawZeroPoint/dzp-dev

Dzp dev

CMakeLists.txt

@@ -69,6 +69,8 @@ endif ()
 find_library(CARTESIO_LIB CartesianInterface HINTS /opt/xbot/)
 if (CARTESIO_LIB)
   message(STATUS "Found CARTESIO_LIB ${CARTESIO_LIB}")
+  find_package(OpenSoT REQUIRED)
+  find_package(cartesian_interface REQUIRED)
 else ()
   message(WARNING "CARTESIO_LIB not found")
 endif ()
@@ -97,6 +99,7 @@ add_service_files(
   FILES
   ExecuteAddCollisionBox.srv
   ExecuteAddCollisionPlane.srv
+  ExecuteAllLockedPoses.srv
   ExecuteAllPlans.srv
   ExecuteAllPoses.srv
   ExecuteAttachCollisionBox.srv
@@ -166,8 +169,8 @@ generate_messages(
 # )
 
 catkin_package(
-  #  INCLUDE_DIRS include
-  LIBRARIES
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
   CATKIN_DEPENDS ${ROS_DEPENDENCIES}
 )
 
@@ -223,14 +226,17 @@ if (HPP_CORE_LIB AND HPP_FCL_LIB)
 endif ()
 
 if (CARTESIO_LIB)
-  include_directories(/opt/xbot/include)
-  add_library(${PROJECT_NAME}_CARTESIO_ADDON
-    src/lib/cartesio_server.cpp
-    )
-  target_link_libraries(${PROJECT_NAME}_CARTESIO_ADDON
+  include_directories(/opt/xbot/include ${OpenSoT_INCLUDE_DIRS})
+  add_library(${PROJECT_NAME}_cartesio src/lib/cartesio_server.cpp)
+  target_link_libraries(${PROJECT_NAME}_cartesio ${catkin_LIBRARIES})
+  add_dependencies(${PROJECT_NAME}_cartesio ${PROJECT_NAME}_generate_messages_cpp)
+
+  add_library(${PROJECT_NAME}_cartesio_addon src/lib/cartesio_addon.cpp)
+  add_dependencies(${PROJECT_NAME}_cartesio_addon ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(${PROJECT_NAME}_cartesio_addon
     ${catkin_LIBRARIES}
-    )
-  add_dependencies(${PROJECT_NAME}_CARTESIO_ADDON ${PROJECT_NAME}_generate_messages_cpp)
+    ${OpenSoT_LIBRARIES}
+    ${cartesian_interface_LIBRARIES})
 endif ()
 
 #####################
@@ -288,7 +294,7 @@ if (CARTESIO_LIB)
   target_link_libraries(roport_cartesio_server
     ${catkin_LIBRARIES}
     ${PROJECT_NAME}
-    ${PROJECT_NAME}_CARTESIO_ADDON
+    ${PROJECT_NAME}_cartesio
     )
 endif ()
 
@@ -301,12 +307,42 @@ catkin_install_python(PROGRAMS
   ${PY_SOURCES}
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
+# Mark executables for installation
+install(TARGETS
+  roport_task_scheduler
+  roport_robot_interface
+  roport_moveit_cpp_server
+  roport_msg_converter
+  roport_msg_aggregator
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+if (CARTESIO_LIB)
+  install(TARGETS roport_cartesio_server
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+    )
+endif ()
+
 install(DIRECTORY
   launch
   tree
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   )
 
+# Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "common.h"
+  PATTERN ".svn" EXCLUDE
+  )
+
+# Mark libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
 #############
 ## Testing ##
 #############

README.md

@@ -2,7 +2,10 @@
 
 <img alt="logo" height="120" src="misc/image/ms-icon-310x310.png" width="120"/>
 
-[![CI](https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml/badge.svg?branch=master)](https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml)
+<p align="left">
+<a href="https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml"><img alt="CI" src="https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml/badge.svg?branch=master"></a>
+<a href="https://github.com/psf/black"><img alt="Code style: black" src="https://img.shields.io/badge/code%20style-black-000000.svg"></a>
+</p>
 
 RoTools is an all-in-one ROS package for high-level robotic task scheduling, visual perception, path planning,
 simulation, and direct-/tele-manipulation control. It leverages BehaviorTree to deliver fast task construction and

include/roport/cartesio_server.h

@@ -43,6 +43,7 @@
 #include <cartesian_interface/ReachPoseAction.h>
 #include <trajectory_msgs/JointTrajectory.h>
 
+#include <roport/ExecuteAllLockedPoses.h>
 #include <roport/ExecuteAllPoses.h>
 #include <roport/ExecuteMirroredPose.h>
 
@@ -64,16 +65,29 @@ class CartesIOServer {
   tf2_ros::TransformListener tf_listener_;
 
   ros::ServiceServer execute_all_poses_srv_;
-  ros::ServiceServer execute_mirrored_pose_srv_;
+  ros::ServiceServer execute_all_locked_poses_srv_;
 
   using reachPoseActionClient = actionlib::SimpleActionClient<cartesian_interface::ReachPoseAction>;
   std::vector<std::shared_ptr<reachPoseActionClient>> control_clients_;
 
+  /**
+   * Move the groups to corresponding poses.
+   * @param req
+   * @param resp
+   * @return
+   */
   auto executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req, roport::ExecuteAllPoses::Response& resp) -> bool;
-  //  auto executeMirroredPoseSrvCb(roport::ExecuteMirroredPose::Request& req, roport::ExecuteMirroredPose::Response&
-  //  resp)
-  //      -> bool;
-  //
+
+  /**
+   * Move the groups to corresponding poses. If only one group is given, this is identical with executeAllPosesSrvCb,
+   * if multiple groups are given, the first group's pose will be the target, while other groups will move along with
+   * the first and keep their relative poses unchanged during the movement.
+   * @param req
+   * @param resp
+   * @return
+   */
+  auto executeAllLockedPosesSrvCb(roport::ExecuteAllLockedPoses::Request& req,
+                                  roport::ExecuteAllLockedPoses::Response& resp) -> bool;
 
   void buildActionGoal(const int& index,
                        const geometry_msgs::Pose& goal_pose,
@@ -84,12 +98,23 @@ 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);
+
+  /**
+   * 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,
+                                const geometry_msgs::Pose& goal_ref_pose,
+                                const int& idx,
+                                const geometry_msgs::Pose& curr_pose,
+                                geometry_msgs::Pose& goal_pose);
 };
 
 }  // namespace roport

include/roport/common.h

@@ -53,6 +53,14 @@ inline auto getParam(const ros::NodeHandle& node_handle,
   return true;
 }
 
+inline auto getIndex(const std::vector<std::string>& names, const std::string& target) -> int {
+  auto res = std::find(names.begin(), names.end(), target);
+  if (res != names.end()) {
+    return res - names.begin();
+  }
+  return -1;
+}
+
 inline void geometryPoseToEigenMatrix(const geometry_msgs::Pose& pose, Eigen::Matrix4d& mat) {
   mat = Eigen::Matrix4d::Identity();
 
@@ -136,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)");
@@ -149,6 +175,60 @@ inline auto isPoseLegal(const geometry_msgs::Pose& pose) -> bool {
   return true;
 }
 
+/**
+ * Converting a quaternion representing a planar rotation to theta.
+ * @param quat Quaternion msg.
+ * @return theta.
+ */
+inline double quaternionToTheta(const geometry_msgs::Quaternion& quat) {
+  Eigen::Quaterniond quaternion(quat.w, quat.x, quat.y, quat.z);
+  Eigen::Rotation2Dd rot(quaternion.toRotationMatrix().topLeftCorner<2, 2>());
+  return rot.smallestPositiveAngle();
+}
+
+/**
+ * Pretty print a Eigen matrix. Its size, row number, and column number will also be displayed.
+ * @param mat A matrix with various shape to print.
+ * @param precision How many digits to keep after the decimal point.
+ */
+inline void prettyPrintEigenMatrix(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& mat,
+                                   const int& precision = 3) {
+  Eigen::IOFormat cleanFormat(precision, 0, ", ", "\n", "[", "]");
+  std::cout << "Matrix size: " << mat.size() << " rows: " << mat.rows() << " cols: " << mat.cols() << std::endl;
+  std::cout << mat.format(cleanFormat) << std::endl;
+}
+
+inline void getCartesianError(const Eigen::Vector3d& current_position,
+                              const Eigen::Quaterniond& current_orientation,
+                              const Eigen::Vector3d& goal_position,
+                              const Eigen::Quaterniond& goal_orientation,
+                              Eigen::Matrix<double, 6, 1>& error) {
+  error.head(3) << current_position - goal_position;
+
+  Eigen::Affine3d current_affine_transform;
+  current_affine_transform.translation() = current_position;
+  current_affine_transform.linear() = current_orientation.toRotationMatrix();
+
+  Eigen::Quaterniond regularized_current_orientation;
+  regularized_current_orientation.coeffs() << current_orientation.coeffs();
+  if (goal_orientation.coeffs().dot(current_orientation.coeffs()) < 0.) {
+    regularized_current_orientation.coeffs() << -current_orientation.coeffs();
+  }
+  Eigen::Quaterniond error_quaternion(regularized_current_orientation.inverse() * goal_orientation);
+  error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
+  error.tail(3) << -current_affine_transform.linear() * error.tail(3);
+}
+
+inline void getCartesianError(const Eigen::Affine3d& current_transform,
+                              const Eigen::Affine3d& goal_transform,
+                              Eigen::Matrix<double, 6, 1>& error) {
+  Eigen::Vector3d current_position(current_transform.translation());
+  Eigen::Quaterniond current_orientation(current_transform.linear());
+  Eigen::Vector3d goal_position(goal_transform.translation());
+  Eigen::Quaterniond goal_orientation(goal_transform.linear());
+  getCartesianError(current_position, current_orientation, goal_position, goal_orientation, error);
+}
+
 /**
  * Judge if all corresponding elements in the two given vectors are close to each other under the tolerance.
  * @tparam T Value type.

scripts/roport_flask_server_template.py

@@ -17,11 +17,11 @@
 # Custom functions
 # TODO add this
 
-'''
+"""
 Note that this is a template for guiding the usage of Flask server
 for porting the algorithm using Python3. You could implement the
 server for your algorithm by referring this.
-'''
+"""
 
 app = Flask(__name__, template_folder="templates")
 
@@ -49,14 +49,18 @@ def decode_b64_to_image(b64_str, is_bgr=True):
         img = base64.b64decode(b64_str)
         # imdecode use the same flag as imread. cf. https://docs.opencv.org/3.4/d8/d6a/group__imgcodecs__flags.html
         if is_bgr:
-            return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR)
+            return True, cv2.imdecode(
+                np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR
+            )
         else:
-            return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH)
+            return True, cv2.imdecode(
+                np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH
+            )
     except cv2.error:
         return False, None
 
 
-@app.route('/process', methods=['POST'])
+@app.route("/process", methods=["POST"])
 def process():
     """The main entrance of the server.
 
@@ -65,34 +69,34 @@ def process():
     """
     try:
         req_data = json.loads(request.data)
-        if 'body' not in req_data:
+        if "body" not in req_data:
             return make_response("Failed to load request data", 200)
         else:
-            req_body = req_data['body']
+            req_body = req_data["body"]
     except ValueError:
         return make_response("Failed to phase JSON", 200)
 
     header = {}
-    response = {'status': False, 'results': []}
+    response = {"status": False, "results": []}
 
     cv_image_list = []
-    image_strings = json.loads(req_body['images'])
+    image_strings = json.loads(req_body["images"])
     for s in image_strings:
         ok, image = decode_b64_to_image(s)
         if not ok:
-            feedback = {'header': header, 'response': response}
+            feedback = {"header": header, "response": response}
             return make_response(json.dumps(feedback), 200)
         cv_image_list.append(image)
 
     ok, results = get_results(cv_image_list)
     torch.cuda.empty_cache()
 
     if ok:
-        response['status'] = True
-        response['results'] = json.dumps(results)
-    feedback = {'header': header, 'response': response}
+        response["status"] = True
+        response["results"] = json.dumps(results)
+    feedback = {"header": header, "response": response}
     return make_response(json.dumps(feedback), 200)
 
 
 if __name__ == "__main__":
-    waitress.serve(app, host='0.0.0.0', port=6060, threads=6)
+    waitress.serve(app, host="0.0.0.0", port=6060, threads=6)