Merge pull request #59 from ethz-asl/feature/remove_mav_msgs

Remove mav_msgs dependency.
ethz-asl · Sep 15, 2021 · 14951b9 · 14951b9
2 parents 040094c + 8b7fe09
commit 14951b9
Show file tree

Hide file tree

Showing 6 changed files with 77 additions and 103 deletions.
diff --git a/install/dependencies.rosinstall b/install/dependencies.rosinstall
@@ -1,3 +1,3 @@
 - git: {local-name: catkin_simple, uri: 'git@github.com:catkin/catkin_simple.git', version: 0e62848b12da76c8cc58a1add42b4f894d1ac21e}
 - git: {local-name: cgal_catkin, uri: 'git@github.com:ethz-asl/cgal_catkin.git', version: releases/CGAL-5.0.3}
-- git: {local-name: polygon_coverage_planning, uri: 'git@github.com:ethz-asl/polygon_coverage_planning.git', version: v2.0.2}
+- git: {local-name: polygon_coverage_planning, uri: 'git@github.com:ethz-asl/polygon_coverage_planning.git', version: v2.0.3}
diff --git a/install/dependencies_https.rosinstall b/install/dependencies_https.rosinstall
@@ -1,3 +1,3 @@
 - git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple.git', version: 0e62848b12da76c8cc58a1add42b4f894d1ac21e}
 - git: {local-name: cgal_catkin, uri: 'https://github.com/ethz-asl/cgal_catkin.git', version: releases/CGAL-5.0.3}
-- git: {local-name: polygon_coverage_planning, uri: 'https://github.com/ethz-asl/polygon_coverage_planning.git', version: v2.0.2}
+- git: {local-name: polygon_coverage_planning, uri: 'https://github.com/ethz-asl/polygon_coverage_planning.git', version: v2.0.3}
diff --git a/install/prepare-jenkins-slave.sh b/install/prepare-jenkins-slave.sh
@@ -7,8 +7,6 @@ echo "ROS version: ${ROS_VERSION}"
 sudo apt-get install -y python-wstool python-catkin-tools
 
 # Package dependencies.
-echo "Installing MAV_COMM dependencies."
-sudo apt-get install -y ros-${ROS_VERSION}-mav-msgs
 echo "Installing CGAL dependencies."
 sudo apt-get install -y libgmp-dev libmpfr-dev
 echo "Installing MONO dependencies."

diff --git a/polygon_coverage_ros/include/polygon_coverage_ros/ros_interface.h b/polygon_coverage_ros/include/polygon_coverage_ros/ros_interface.h
@@ -25,15 +25,13 @@
 
 #include <eigen_conversions/eigen_msg.h>
 #include <geometry_msgs/Polygon.h>
+#include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseArray.h>
-#include <mav_msgs/conversions.h>
 #include <polygon_coverage_msgs/PolygonWithHolesStamped.h>
 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
 
-#include <mav_msgs/eigen_mav_msgs.h>
-
 #include <polygon_coverage_geometry/cgal_definitions.h>
 
 namespace polygon_coverage_planning {
@@ -64,15 +62,6 @@ class Color : public std_msgs::ColorRGBA {
 };
 
 // Warning: Does not set frame or time stamps or orientation.
-void eigenTrajectoryPointVectorFromPath(
-    const std::vector<Point_2>& waypoints, double altitude,
-    mav_msgs::EigenTrajectoryPointVector* traj_points);
-
-void poseArrayMsgFromEigenTrajectoryPointVector(
-    const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
-    const std::string& frame_id,
-    geometry_msgs::PoseArray* trajectory_points_pose_array);
-
 void poseArrayMsgFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     const std::string& frame_id,
@@ -82,6 +71,9 @@ void msgMultiDofJointTrajectoryFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     trajectory_msgs::MultiDOFJointTrajectory* msg);
 
+void msgPointFromWaypoint(const Point_2& waypoint, double altitude,
+                          geometry_msgs::Point* point);
+
 void createMarkers(const std::vector<Point_2>& vertices, double altitude,
                    const std::string& frame_id, const std::string& ns,
                    const Color& points_color, const Color& lines_color,
@@ -106,8 +98,8 @@ void createStartAndEndPointMarkers(const Point_2& start, const Point_2& end,
                                    visualization_msgs::Marker* start_point,
                                    visualization_msgs::Marker* end_point);
 
-void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
-                                   const mav_msgs::EigenTrajectoryPoint& end,
+void createStartAndEndPointMarkers(const geometry_msgs::Pose& start,
+                                   const geometry_msgs::Pose& end,
                                    const std::string& frame_id,
                                    const std::string& ns,
                                    visualization_msgs::Marker* start_point,
@@ -119,8 +111,8 @@ void createStartAndEndTextMarkers(const Point_2& start, const Point_2& end,
                                   visualization_msgs::Marker* start_text,
                                   visualization_msgs::Marker* end_text);
 
-void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
-                                  const mav_msgs::EigenTrajectoryPoint& end,
+void createStartAndEndTextMarkers(const geometry_msgs::Pose& start,
+                                  const geometry_msgs::Pose& end,
                                   const std::string& frame_id,
                                   const std::string& ns,
                                   visualization_msgs::Marker* start_text,

diff --git a/polygon_coverage_ros/package.xml b/polygon_coverage_ros/package.xml
@@ -16,7 +16,6 @@
   <depend>roscpp</depend>
 
   <depend>nav_msgs</depend>
-  <depend>mav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>visualization_msgs</depend>

diff --git a/polygon_coverage_ros/src/ros_interface.cc b/polygon_coverage_ros/src/ros_interface.cc
@@ -35,57 +35,56 @@
 
 namespace polygon_coverage_planning {
 
-void eigenTrajectoryPointVectorFromPath(
-    const std::vector<Point_2>& waypoints, double altitude,
-    mav_msgs::EigenTrajectoryPointVector* traj_points) {
-  ROS_ASSERT(traj_points);
-
-  traj_points->clear();
-  traj_points->resize(waypoints.size());
+void poseArrayMsgFromPath(const std::vector<Point_2>& waypoints,
+                          double altitude, const std::string& frame_id,
+                          geometry_msgs::PoseArray* pose_array) {
+  ROS_ASSERT(pose_array);
+
+  pose_array->header.frame_id = frame_id;
+  pose_array->poses.clear();
+  pose_array->poses.resize(waypoints.size());
   for (size_t i = 0; i < waypoints.size(); i++) {
-    (*traj_points)[i].position_W =
-        Eigen::Vector3d(CGAL::to_double(waypoints[i].x()),
-                        CGAL::to_double(waypoints[i].y()), altitude);
+    msgPointFromWaypoint(waypoints[i], altitude,
+                         &pose_array->poses[i].position);
   }
 }
 
-void poseArrayMsgFromEigenTrajectoryPointVector(
-    const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
-    const std::string& frame_id,
-    geometry_msgs::PoseArray* trajectory_points_pose_array) {
-  ROS_ASSERT(trajectory_points_pose_array);
-
-  // Header
-  trajectory_points_pose_array->header.frame_id = frame_id;
-  // Converting and populating the message with all points
-  for (const mav_msgs::EigenTrajectoryPoint& trajectory_point :
-       trajectory_points) {
-    geometry_msgs::PoseStamped msg;
-    mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(trajectory_point, &msg);
-    trajectory_points_pose_array->poses.push_back(msg.pose);
-  }
-}
-
-void poseArrayMsgFromPath(
-    const std::vector<Point_2>& waypoints, double altitude,
-    const std::string& frame_id,
-    geometry_msgs::PoseArray* trajectory_points_pose_array) {
-  ROS_ASSERT(trajectory_points_pose_array);
-
-  mav_msgs::EigenTrajectoryPointVector eigen_traj;
-  eigenTrajectoryPointVectorFromPath(waypoints, altitude, &eigen_traj);
-  poseArrayMsgFromEigenTrajectoryPointVector(eigen_traj, frame_id,
-                                             trajectory_points_pose_array);
-}
-
 void msgMultiDofJointTrajectoryFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     trajectory_msgs::MultiDOFJointTrajectory* msg) {
   ROS_ASSERT(msg);
 
-  mav_msgs::EigenTrajectoryPointVector eigen_traj;
-  eigenTrajectoryPointVectorFromPath(waypoints, altitude, &eigen_traj);
-  mav_msgs::msgMultiDofJointTrajectoryFromEigen(eigen_traj, msg);
+  geometry_msgs::PoseArray pose_array;
+  poseArrayMsgFromPath(waypoints, altitude, "", &pose_array);
+
+  msg->header = pose_array.header;
+  msg->joint_names.clear();
+  msg->joint_names.push_back("base_link");
+  msg->points.clear();
+
+  for (const auto& pose : pose_array.poses) {
+    trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
+    point_msg.transforms.resize(1);
+    point_msg.velocities.resize(1);
+    point_msg.accelerations.resize(1);
+
+    point_msg.transforms[0].translation.x = pose.position.x;
+    point_msg.transforms[0].translation.y = pose.position.y;
+    point_msg.transforms[0].translation.z = pose.position.z;
+
+    point_msg.transforms[0].rotation.w = 1.0;
+
+    msg->points.push_back(point_msg);
+  }
+}
+
+void msgPointFromWaypoint(const Point_2& waypoint, double altitude,
+                          geometry_msgs::Point* point) {
+  ROS_ASSERT(point);
+
+  point->x = CGAL::to_double(waypoint.x());
+  point->y = CGAL::to_double(waypoint.y());
+  point->z = altitude;
 }
 
 void createMarkers(const std::vector<Point_2>& vertices, double altitude,
@@ -120,9 +119,7 @@ void createMarkers(const std::vector<Point_2>& vertices, double altitude,
 
   for (size_t i = 0; i < vertices.size(); i++) {
     geometry_msgs::Point p;
-    p.x = CGAL::to_double(vertices[i].x());
-    p.y = CGAL::to_double(vertices[i].y());
-    p.z = altitude;
+    msgPointFromWaypoint(vertices[i], altitude, &p);
 
     points->points.push_back(p);
     line_strip->points.push_back(p);
@@ -167,22 +164,21 @@ void createStartAndEndPointMarkers(const Point_2& start, const Point_2& end,
                                    const std::string& ns,
                                    visualization_msgs::Marker* start_point,
                                    visualization_msgs::Marker* end_point) {
-  mav_msgs::EigenTrajectoryPoint eigen_start;
-  eigen_start.position_W.x() = CGAL::to_double(start.x());
-  eigen_start.position_W.y() = CGAL::to_double(start.y());
-  eigen_start.position_W.z() = altitude;
+  ROS_ASSERT(start_point);
+  ROS_ASSERT(end_point);
 
-  mav_msgs::EigenTrajectoryPoint eigen_end;
-  eigen_end.position_W.x() = CGAL::to_double(end.x());
-  eigen_end.position_W.y() = CGAL::to_double(end.y());
-  eigen_end.position_W.z() = altitude;
+  geometry_msgs::Pose start_pose;
+  msgPointFromWaypoint(start, altitude, &start_pose.position);
 
-  return createStartAndEndPointMarkers(eigen_start, eigen_end, frame_id, ns,
+  geometry_msgs::Pose end_pose;
+  msgPointFromWaypoint(end, altitude, &end_pose.position);
+
+  return createStartAndEndPointMarkers(start_pose, end_pose, frame_id, ns,
                                        start_point, end_point);
 }
 
-void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
-                                   const mav_msgs::EigenTrajectoryPoint& end,
+void createStartAndEndPointMarkers(const geometry_msgs::Pose& start,
+                                   const geometry_msgs::Pose& end,
                                    const std::string& frame_id,
                                    const std::string& ns,
                                    visualization_msgs::Marker* start_point,
@@ -196,12 +192,8 @@ void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
   end_point->ns = ns + "_end";
   start_point->action = end_point->action = visualization_msgs::Marker::ADD;
 
-  geometry_msgs::PoseStamped start_stamped, end_stamped;
-  msgPoseStampedFromEigenTrajectoryPoint(start, &start_stamped);
-  msgPoseStampedFromEigenTrajectoryPoint(end, &end_stamped);
-
-  start_point->pose = start_stamped.pose;
-  end_point->pose = end_stamped.pose;
+  start_point->pose = start;
+  end_point->pose = end;
 
   start_point->pose.orientation.w = end_point->pose.orientation.w = 1.0;
 
@@ -225,22 +217,21 @@ void createStartAndEndTextMarkers(const Point_2& start, const Point_2& end,
                                   const std::string& ns,
                                   visualization_msgs::Marker* start_text,
                                   visualization_msgs::Marker* end_text) {
-  mav_msgs::EigenTrajectoryPoint eigen_start;
-  eigen_start.position_W.x() = CGAL::to_double(start.x());
-  eigen_start.position_W.y() = CGAL::to_double(start.y());
-  eigen_start.position_W.z() = altitude;
+  ROS_ASSERT(start_text);
+  ROS_ASSERT(end_text);
+
+  geometry_msgs::Pose start_pose;
+  msgPointFromWaypoint(start, altitude, &start_pose.position);
 
-  mav_msgs::EigenTrajectoryPoint eigen_end;
-  eigen_end.position_W.x() = CGAL::to_double(end.x());
-  eigen_end.position_W.y() = CGAL::to_double(end.y());
-  eigen_end.position_W.z() = altitude;
+  geometry_msgs::Pose end_pose;
+  msgPointFromWaypoint(end, altitude, &end_pose.position);
 
-  return createStartAndEndTextMarkers(eigen_start, eigen_end, frame_id, ns,
+  return createStartAndEndTextMarkers(start_pose, end_pose, frame_id, ns,
                                       start_text, end_text);
 }
 
-void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
-                                  const mav_msgs::EigenTrajectoryPoint& end,
+void createStartAndEndTextMarkers(const geometry_msgs::Pose& start,
+                                  const geometry_msgs::Pose& end,
                                   const std::string& frame_id,
                                   const std::string& ns,
                                   visualization_msgs::Marker* start_text,
@@ -256,12 +247,8 @@ void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
   start_text->type = end_text->type =
       visualization_msgs::Marker::TEXT_VIEW_FACING;
 
-  geometry_msgs::PoseStamped start_stamped, end_stamped;
-  msgPoseStampedFromEigenTrajectoryPoint(start, &start_stamped);
-  msgPoseStampedFromEigenTrajectoryPoint(end, &end_stamped);
-
-  start_text->pose = start_stamped.pose;
-  end_text->pose = end_stamped.pose;
+  start_text->pose = start;
+  end_text->pose = end;
 
   start_text->pose.orientation.w = end_text->pose.orientation.w = 1.0;
 
@@ -364,9 +351,7 @@ void createTriangles(const std::vector<std::vector<Point_2>>& triangles,
     ROS_ASSERT(t.size() == 3);
     for (const auto& v : t) {
       geometry_msgs::Point p;
-      p.x = CGAL::to_double(v.x());
-      p.y = CGAL::to_double(v.y());
-      p.z = altitude;
+      msgPointFromWaypoint(v, altitude, &p);
 
       markers->points.push_back(p);
     }