Skip to content

Commit 8a31688

Browse files
rikbarikba
committed
Fix issue #46 
1 parent a748295 commit 8a31688

File tree

4 files changed

+64
-95
lines changed

4 files changed

+64
-95
lines changed

‎install/prepare-jenkins-slave.sh‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,6 @@ echo "ROS version: ${ROS_VERSION}"
77
sudo apt-get install -y python-wstool python-catkin-tools
88

99
# Package dependencies.
10-
echo "Installing MAV_COMM dependencies."
11-
sudo apt-get install -y ros-${ROS_VERSION}-mav-msgs
1210
echo "Installing CGAL dependencies."
1311
sudo apt-get install -y libgmp-dev libmpfr-dev
1412
echo "Installing MONO dependencies."

‎polygon_coverage_ros/include/polygon_coverage_ros/ros_interface.h‎

Lines changed: 5 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -25,15 +25,13 @@
2525

2626
#include <eigen_conversions/eigen_msg.h>
2727
#include <geometry_msgs/Polygon.h>
28+
#include <geometry_msgs/Pose.h>
2829
#include <geometry_msgs/PoseArray.h>
29-
#include <mav_msgs/conversions.h>
3030
#include <polygon_coverage_msgs/PolygonWithHolesStamped.h>
3131
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
3232
#include <visualization_msgs/Marker.h>
3333
#include <visualization_msgs/MarkerArray.h>
3434

35-
#include <mav_msgs/eigen_mav_msgs.h>
36-
3735
#include <polygon_coverage_geometry/cgal_definitions.h>
3836

3937
namespace polygon_coverage_planning {
@@ -64,15 +62,6 @@ class Color : public std_msgs::ColorRGBA {
6462
};
6563

6664
// Warning: Does not set frame or time stamps or orientation.
67-
void eigenTrajectoryPointVectorFromPath(
68-
const std::vector<Point_2>& waypoints, double altitude,
69-
mav_msgs::EigenTrajectoryPointVector* traj_points);
70-
71-
void poseArrayMsgFromEigenTrajectoryPointVector(
72-
const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
73-
const std::string& frame_id,
74-
geometry_msgs::PoseArray* trajectory_points_pose_array);
75-
7665
void poseArrayMsgFromPath(
7766
const std::vector<Point_2>& waypoints, double altitude,
7867
const std::string& frame_id,
@@ -106,8 +95,8 @@ void createStartAndEndPointMarkers(const Point_2& start, const Point_2& end,
10695
visualization_msgs::Marker* start_point,
10796
visualization_msgs::Marker* end_point);
10897

109-
void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
110-
const mav_msgs::EigenTrajectoryPoint& end,
98+
void createStartAndEndPointMarkers(const geometry_msgs::Pose& start,
99+
const geometry_msgs::Pose& end,
111100
const std::string& frame_id,
112101
const std::string& ns,
113102
visualization_msgs::Marker* start_point,
@@ -119,8 +108,8 @@ void createStartAndEndTextMarkers(const Point_2& start, const Point_2& end,
119108
visualization_msgs::Marker* start_text,
120109
visualization_msgs::Marker* end_text);
121110

122-
void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
123-
const mav_msgs::EigenTrajectoryPoint& end,
111+
void createStartAndEndTextMarkers(const geometry_msgs::Pose& start,
112+
const geometry_msgs::Pose& end,
124113
const std::string& frame_id,
125114
const std::string& ns,
126115
visualization_msgs::Marker* start_text,

‎polygon_coverage_ros/package.xml‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
<depend>roscpp</depend>
1717

1818
<depend>nav_msgs</depend>
19-
<depend>mav_msgs</depend>
2019
<depend>geometry_msgs</depend>
2120
<depend>trajectory_msgs</depend>
2221
<depend>visualization_msgs</depend>

‎polygon_coverage_ros/src/ros_interface.cc‎

Lines changed: 59 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -35,57 +35,48 @@
3535

3636
namespace polygon_coverage_planning {
3737

38-
void eigenTrajectoryPointVectorFromPath(
39-
const std::vector<Point_2>& waypoints, double altitude,
40-
mav_msgs::EigenTrajectoryPointVector* traj_points) {
41-
ROS_ASSERT(traj_points);
42-
43-
traj_points->clear();
44-
traj_points->resize(waypoints.size());
38+
void poseArrayMsgFromPath(const std::vector<Point_2>& waypoints,
39+
double altitude, const std::string& frame_id,
40+
geometry_msgs::PoseArray* pose_array) {
41+
ROS_ASSERT(pose_array);
42+
43+
pose_array->header.frame_id = frame_id;
44+
pose_array->poses.clear();
45+
pose_array->poses.resize(waypoints.size());
4546
for (size_t i = 0; i < waypoints.size(); i++) {
46-
(*traj_points)[i].position_W =
47-
Eigen::Vector3d(CGAL::to_double(waypoints[i].x()),
48-
CGAL::to_double(waypoints[i].y()), altitude);
49-
}
50-
}
51-
52-
void poseArrayMsgFromEigenTrajectoryPointVector(
53-
const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
54-
const std::string& frame_id,
55-
geometry_msgs::PoseArray* trajectory_points_pose_array) {
56-
ROS_ASSERT(trajectory_points_pose_array);
57-
58-
// Header
59-
trajectory_points_pose_array->header.frame_id = frame_id;
60-
// Converting and populating the message with all points
61-
for (const mav_msgs::EigenTrajectoryPoint& trajectory_point :
62-
trajectory_points) {
63-
geometry_msgs::PoseStamped msg;
64-
mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(trajectory_point, &msg);
65-
trajectory_points_pose_array->poses.push_back(msg.pose);
47+
pose_array->poses[i].position.x = CGAL::to_double(waypoints[i].x());
48+
pose_array->poses[i].position.y = CGAL::to_double(waypoints[i].y());
49+
pose_array->poses[i].position.z = altitude;
6650
}
6751
}
6852

69-
void poseArrayMsgFromPath(
70-
const std::vector<Point_2>& waypoints, double altitude,
71-
const std::string& frame_id,
72-
geometry_msgs::PoseArray* trajectory_points_pose_array) {
73-
ROS_ASSERT(trajectory_points_pose_array);
74-
75-
mav_msgs::EigenTrajectoryPointVector eigen_traj;
76-
eigenTrajectoryPointVectorFromPath(waypoints, altitude, &eigen_traj);
77-
poseArrayMsgFromEigenTrajectoryPointVector(eigen_traj, frame_id,
78-
trajectory_points_pose_array);
79-
}
80-
8153
void msgMultiDofJointTrajectoryFromPath(
8254
const std::vector<Point_2>& waypoints, double altitude,
8355
trajectory_msgs::MultiDOFJointTrajectory* msg) {
8456
ROS_ASSERT(msg);
8557

86-
mav_msgs::EigenTrajectoryPointVector eigen_traj;
87-
eigenTrajectoryPointVectorFromPath(waypoints, altitude, &eigen_traj);
88-
mav_msgs::msgMultiDofJointTrajectoryFromEigen(eigen_traj, msg);
58+
geometry_msgs::PoseArray pose_array;
59+
poseArrayMsgFromPath(waypoints, altitude, "", &pose_array);
60+
61+
msg->header = pose_array.header;
62+
msg->joint_names.clear();
63+
msg->joint_names.push_back("base_link");
64+
msg->points.clear();
65+
66+
for (const auto& pose : pose_array.poses) {
67+
trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
68+
point_msg.transforms.resize(1);
69+
point_msg.velocities.resize(1);
70+
point_msg.accelerations.resize(1);
71+
72+
point_msg.transforms[0].translation.x = pose.position.x;
73+
point_msg.transforms[0].translation.y = pose.position.y;
74+
point_msg.transforms[0].translation.z = pose.position.z;
75+
76+
point_msg.transforms[0].rotation.w = 1.0;
77+
78+
msg->points.push_back(point_msg);
79+
}
8980
}
9081

9182
void createMarkers(const std::vector<Point_2>& vertices, double altitude,
@@ -167,22 +158,22 @@ void createStartAndEndPointMarkers(const Point_2& start, const Point_2& end,
167158
const std::string& ns,
168159
visualization_msgs::Marker* start_point,
169160
visualization_msgs::Marker* end_point) {
170-
mav_msgs::EigenTrajectoryPoint eigen_start;
171-
eigen_start.position_W.x() = CGAL::to_double(start.x());
172-
eigen_start.position_W.y() = CGAL::to_double(start.y());
173-
eigen_start.position_W.z() = altitude;
161+
geometry_msgs::Pose start_pose;
162+
start_pose.position.x = CGAL::to_double(start.x());
163+
start_pose.position.y = CGAL::to_double(start.y());
164+
start_pose.position.z = altitude;
174165

175-
mav_msgs::EigenTrajectoryPoint eigen_end;
176-
eigen_end.position_W.x() = CGAL::to_double(end.x());
177-
eigen_end.position_W.y() = CGAL::to_double(end.y());
178-
eigen_end.position_W.z() = altitude;
166+
geometry_msgs::Pose end_pose;
167+
end_pose.position.x = CGAL::to_double(end.x());
168+
end_pose.position.y = CGAL::to_double(end.y());
169+
end_pose.position.z = altitude;
179170

180-
return createStartAndEndPointMarkers(eigen_start, eigen_end, frame_id, ns,
171+
return createStartAndEndPointMarkers(start_pose, end_pose, frame_id, ns,
181172
start_point, end_point);
182173
}
183174

184-
void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
185-
const mav_msgs::EigenTrajectoryPoint& end,
175+
void createStartAndEndPointMarkers(const geometry_msgs::Pose& start,
176+
const geometry_msgs::Pose& end,
186177
const std::string& frame_id,
187178
const std::string& ns,
188179
visualization_msgs::Marker* start_point,
@@ -196,12 +187,8 @@ void createStartAndEndPointMarkers(const mav_msgs::EigenTrajectoryPoint& start,
196187
end_point->ns = ns + "_end";
197188
start_point->action = end_point->action = visualization_msgs::Marker::ADD;
198189

199-
geometry_msgs::PoseStamped start_stamped, end_stamped;
200-
msgPoseStampedFromEigenTrajectoryPoint(start, &start_stamped);
201-
msgPoseStampedFromEigenTrajectoryPoint(end, &end_stamped);
202-
203-
start_point->pose = start_stamped.pose;
204-
end_point->pose = end_stamped.pose;
190+
start_point->pose = start;
191+
end_point->pose = end;
205192

206193
start_point->pose.orientation.w = end_point->pose.orientation.w = 1.0;
207194

@@ -225,22 +212,22 @@ void createStartAndEndTextMarkers(const Point_2& start, const Point_2& end,
225212
const std::string& ns,
226213
visualization_msgs::Marker* start_text,
227214
visualization_msgs::Marker* end_text) {
228-
mav_msgs::EigenTrajectoryPoint eigen_start;
229-
eigen_start.position_W.x() = CGAL::to_double(start.x());
230-
eigen_start.position_W.y() = CGAL::to_double(start.y());
231-
eigen_start.position_W.z() = altitude;
215+
geometry_msgs::Pose start_pose;
216+
start_pose.position.x = CGAL::to_double(start.x());
217+
start_pose.position.y = CGAL::to_double(start.y());
218+
start_pose.position.z = altitude;
232219

233-
mav_msgs::EigenTrajectoryPoint eigen_end;
234-
eigen_end.position_W.x() = CGAL::to_double(end.x());
235-
eigen_end.position_W.y() = CGAL::to_double(end.y());
236-
eigen_end.position_W.z() = altitude;
220+
geometry_msgs::Pose end_pose;
221+
end_pose.position.x = CGAL::to_double(end.x());
222+
end_pose.position.y = CGAL::to_double(end.y());
223+
end_pose.position.z = altitude;
237224

238-
return createStartAndEndTextMarkers(eigen_start, eigen_end, frame_id, ns,
225+
return createStartAndEndTextMarkers(start_pose, end_pose, frame_id, ns,
239226
start_text, end_text);
240227
}
241228

242-
void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
243-
const mav_msgs::EigenTrajectoryPoint& end,
229+
void createStartAndEndTextMarkers(const geometry_msgs::Pose& start,
230+
const geometry_msgs::Pose& end,
244231
const std::string& frame_id,
245232
const std::string& ns,
246233
visualization_msgs::Marker* start_text,
@@ -256,12 +243,8 @@ void createStartAndEndTextMarkers(const mav_msgs::EigenTrajectoryPoint& start,
256243
start_text->type = end_text->type =
257244
visualization_msgs::Marker::TEXT_VIEW_FACING;
258245

259-
geometry_msgs::PoseStamped start_stamped, end_stamped;
260-
msgPoseStampedFromEigenTrajectoryPoint(start, &start_stamped);
261-
msgPoseStampedFromEigenTrajectoryPoint(end, &end_stamped);
262-
263-
start_text->pose = start_stamped.pose;
264-
end_text->pose = end_stamped.pose;
246+
start_text->pose = start;
247+
end_text->pose = end;
265248

266249
start_text->pose.orientation.w = end_text->pose.orientation.w = 1.0;
267250

0 commit comments

Comments
 (0)