From 16c135d8e931b4b4590479c7d493b300763ae0e7 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 13 Dec 2017 14:02:02 +0100 Subject: [PATCH 1/4] plan_route_plugin: waypoints_ relative to target_frame --- mapviz_plugins/src/plan_route_plugin.cpp | 169 ++++++++++------------- 1 file changed, 73 insertions(+), 96 deletions(-) diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp index a3c4c581..f1172285 100644 --- a/mapviz_plugins/src/plan_route_plugin.cpp +++ b/mapviz_plugins/src/plan_route_plugin.cpp @@ -126,11 +126,28 @@ namespace mapviz_plugins ros::ServiceClient client = node_.serviceClient(service); mnm::PlanRoute plan_route; - plan_route.request.header.frame_id = stu::_wgs84_frame; plan_route.request.header.stamp = ros::Time::now(); plan_route.request.plan_from_vehicle = static_cast(start_from_vehicle); plan_route.request.waypoints = waypoints_; + stu::Transform transform; + if (tf_manager_.GetTransform(stu::_wgs84_frame, target_frame_, transform)) + { + plan_route.request.header.frame_id = stu::_wgs84_frame; + + for(int i=0; i::max(); - QPointF point = event->localPos(); - stu::Transform transform; - if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) - { - for (size_t i = 0; i < waypoints_.size(); i++) - { - tf::Vector3 waypoint( - waypoints_[i].position.x, - waypoints_[i].position.y, - 0.0); - waypoint = transform * waypoint; - - QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(waypoint.x(), waypoint.y())); + QPointF mouse_pos = event->localPos(); - double distance = QLineF(transformed, point).length(); + for (size_t i = 0; i < waypoints_.size(); i++) + { + QPointF waypoint(waypoints_[i].position.x, waypoints_[i].position.y); + QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(waypoint); + double distance = QLineF(transformed, mouse_pos).length(); if (distance < closest_distance) { - closest_distance = distance; - closest_point = static_cast(i); + closest_distance = distance; + closest_point = static_cast(i); } - } } if (event->button() == Qt::LeftButton) @@ -272,17 +281,10 @@ namespace mapviz_plugins { if (selected_point_ >= 0 && static_cast(selected_point_) < waypoints_.size()) { - QPointF point = event->localPos(); - stu::Transform transform; - if (tf_manager_.GetTransform(stu::_wgs84_frame, target_frame_, transform)) - { - QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); - tf::Vector3 position(transformed.x(), transformed.y(), 0.0); - position = transform * position; - waypoints_[selected_point_].position.x = position.x(); - waypoints_[selected_point_].position.y = position.y(); - PlanRoute(); - } + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(event->localPos()); + waypoints_[selected_point_].position.x = transformed.x(); + waypoints_[selected_point_].position.y = transformed.y(); + PlanRoute(); selected_point_ = -1; return true; @@ -298,23 +300,12 @@ namespace mapviz_plugins // or just holding the cursor in place. if (msecsDiff < max_ms_ && distance <= max_distance_) { - QPointF point = event->localPos(); - - - QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); - - stu::Transform transform; - tf::Vector3 position(transformed.x(), transformed.y(), 0.0); - if (tf_manager_.GetTransform(stu::_wgs84_frame, target_frame_, transform)) - { - position = transform * position; - - geometry_msgs::Pose pose; - pose.position.x = position.x(); - pose.position.y = position.y(); - waypoints_.push_back(pose); - PlanRoute(); - } + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(event->localPos()); + geometry_msgs::Pose pose; + pose.position.x = transformed.x(); + pose.position.y = transformed.y(); + waypoints_.push_back(pose); + PlanRoute(); } } is_mouse_down_ = false; @@ -326,18 +317,10 @@ namespace mapviz_plugins { if (selected_point_ >= 0 && static_cast(selected_point_) < waypoints_.size()) { - QPointF point = event->localPos(); - stu::Transform transform; - if (tf_manager_.GetTransform(stu::_wgs84_frame, target_frame_, transform)) - { - QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); - tf::Vector3 position(transformed.x(), transformed.y(), 0.0); - position = transform * position; - waypoints_[selected_point_].position.y = position.y(); - waypoints_[selected_point_].position.x = position.x(); - PlanRoute(); - } - + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(event->localPos()); + waypoints_[selected_point_].position.y = transformed.y(); + waypoints_[selected_point_].position.x = transformed.x(); + PlanRoute(); return true; } return false; @@ -345,52 +328,51 @@ namespace mapviz_plugins void PlanRoutePlugin::Draw(double x, double y, double scale) { - stu::Transform transform; - if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) + if (!failed_service_) { - if (!failed_service_) + if (route_preview_) { - if (route_preview_) - { - sru::Route route = *route_preview_; - sru::transform(route, transform, target_frame_); + glLineWidth(2); + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); - glLineWidth(2); - const QColor color = ui_.color->color(); - glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); - glBegin(GL_LINE_STRIP); + sru::Route route = *route_preview_; + stu::Transform transform; + glBegin(GL_LINE_STRIP); + if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) + { + sru::transform(route, transform, target_frame_); for (size_t i = 0; i < route.points.size(); i++) { glVertex2d(route.points[i].position().x(), route.points[i].position().y()); } - - glEnd(); + PrintInfo("OK"); } - - PrintInfo("OK"); + else{ + for (size_t i = 0; i < waypoints_.size(); i++) + { + glVertex2d(waypoints_[i].position.x, waypoints_[i].position.y); + } + PrintError("Route failed to convert to wgs84"); + } + glEnd(); } + } - // Draw waypoints - - glPointSize(20); - glColor4f(0.0, 1.0, 1.0, 1.0); - glBegin(GL_POINTS); + // Draw waypoints + glPointSize(20); + glColor4f(0.0, 1.0, 1.0, 1.0); + glBegin(GL_POINTS); - for (size_t i = 0; i < waypoints_.size(); i++) - { - tf::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0); - point = transform * point; - glVertex2d(point.x(), point.y()); - } - glEnd(); - } - else + for (size_t i = 0; i < waypoints_.size(); i++) { - PrintError("Failed to transform."); + glVertex2d(waypoints_[i].position.x, waypoints_[i].position.y); } + glEnd(); } + void PlanRoutePlugin::Paint(QPainter* painter, double x, double y, double scale) { painter->save(); @@ -400,18 +382,13 @@ namespace mapviz_plugins painter->setPen(pen); painter->setFont(QFont("DejaVu Sans Mono", 7)); - stu::Transform transform; - if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) + for (size_t i = 0; i < waypoints_.size(); i++) { - for (size_t i = 0; i < waypoints_.size(); i++) - { - tf::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0); - point = transform * point; - QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(QPointF(point.x(), point.y())); + QPointF point(waypoints_[i].position.x, waypoints_[i].position.y); + QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(point); QPointF corner(gl_point.x() - 20, gl_point.y() - 20); QRectF rect(corner, QSizeF(40, 40)); painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::fromStdString(boost::lexical_cast(i + 1))); - } } painter->restore(); From 377fa53d4c00afe7ae569020a8e8da72cd10e135 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 13 Dec 2017 14:22:32 +0100 Subject: [PATCH 2/4] more c++11 --- mapviz_plugins/src/plan_route_plugin.cpp | 63 ++++++++++++------------ 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp index f1172285..ce6f9d87 100644 --- a/mapviz_plugins/src/plan_route_plugin.cpp +++ b/mapviz_plugins/src/plan_route_plugin.cpp @@ -145,7 +145,7 @@ namespace mapviz_plugins } else{ plan_route.request.header.frame_id = target_frame_; - PrintfWarning("PlanRoute sent relative to frame [%s]", target_frame_); + PrintWarning("PlanRoute sent relative to target_frame"); } if (client.call(plan_route)) @@ -328,46 +328,46 @@ namespace mapviz_plugins void PlanRoutePlugin::Draw(double x, double y, double scale) { - if (!failed_service_) + stu::Transform transform; + bool show_server_route = ( !failed_service_ && route_preview_ ); + if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) { - if (route_preview_) - { - glLineWidth(2); - const QColor color = ui_.color->color(); - glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); + show_server_route = false; + } - sru::Route route = *route_preview_; - stu::Transform transform; + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); - glBegin(GL_LINE_STRIP); - if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) - { - sru::transform(route, transform, target_frame_); - for (size_t i = 0; i < route.points.size(); i++) - { - glVertex2d(route.points[i].position().x(), route.points[i].position().y()); - } - PrintInfo("OK"); - } - else{ - for (size_t i = 0; i < waypoints_.size(); i++) - { - glVertex2d(waypoints_[i].position.x, waypoints_[i].position.y); - } - PrintError("Route failed to convert to wgs84"); - } - glEnd(); + glBegin(GL_LINE_STRIP); + if (show_server_route) + { + glLineWidth(2); + sru::Route route = *route_preview_; + sru::transform(route, transform, target_frame_); + for (size_t i = 0; i < route.points.size(); i++) + { + glVertex2d(route.points[i].position().x(), route.points[i].position().y()); } + PrintInfo("OK"); } + else{ + glLineWidth(1); + for (const geometry_msgs::Pose& waypoint: waypoints_) + { + glVertex2d(waypoint.position.x, waypoint.position.y); + } + PrintError("Route failed to convert to wgs84"); + } + glEnd(); // Draw waypoints glPointSize(20); glColor4f(0.0, 1.0, 1.0, 1.0); glBegin(GL_POINTS); - for (size_t i = 0; i < waypoints_.size(); i++) + for (const geometry_msgs::Pose& waypoint: waypoints_) { - glVertex2d(waypoints_[i].position.x, waypoints_[i].position.y); + glVertex2d(waypoint.position.x, waypoint.position.y); } glEnd(); } @@ -382,13 +382,14 @@ namespace mapviz_plugins painter->setPen(pen); painter->setFont(QFont("DejaVu Sans Mono", 7)); - for (size_t i = 0; i < waypoints_.size(); i++) + for (int i=0; i< waypoints_.size(); i++) { QPointF point(waypoints_[i].position.x, waypoints_[i].position.y); QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(point); QPointF corner(gl_point.x() - 20, gl_point.y() - 20); QRectF rect(corner, QSizeF(40, 40)); - painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::fromStdString(boost::lexical_cast(i + 1))); + painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, + QString::fromStdString(std::to_string(i + 1))); } painter->restore(); From a64462641e36296c7ef95a21d9ba37c6b4df92a0 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 13 Dec 2017 16:22:04 +0100 Subject: [PATCH 3/4] minor changes --- mapviz_plugins/src/plan_route_plugin.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp index ce6f9d87..b2886983 100644 --- a/mapviz_plugins/src/plan_route_plugin.cpp +++ b/mapviz_plugins/src/plan_route_plugin.cpp @@ -135,12 +135,12 @@ namespace mapviz_plugins { plan_route.request.header.frame_id = stu::_wgs84_frame; - for(int i=0; idrawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::fromStdString(std::to_string(i + 1))); } - painter->restore(); } From 2d033d68bdb09d640974a8b2762c9995902671c7 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 14 Dec 2017 14:36:45 +0100 Subject: [PATCH 4/4] simplification --- mapviz_plugins/src/plan_route_plugin.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp index b2886983..a7423dca 100644 --- a/mapviz_plugins/src/plan_route_plugin.cpp +++ b/mapviz_plugins/src/plan_route_plugin.cpp @@ -388,8 +388,7 @@ namespace mapviz_plugins QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(point); QPointF corner(gl_point.x() - 20, gl_point.y() - 20); QRectF rect(corner, QSizeF(40, 40)); - painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, - QString::fromStdString(std::to_string(i + 1))); + painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::number(i + 1)); } painter->restore(); }