diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp index a3c4c581a..a7423dca9 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( geometry_msgs::Pose& waypoint: plan_route.request.waypoints ) + { + tf::Vector3 point( waypoint.position.x, waypoint.position.y, 0.0); + point = transform * point; + waypoint.position.x = point.x(); + waypoint.position.y = point.y(); + } + } + else{ + plan_route.request.header.frame_id = target_frame_; + PrintWarning("PlanRoute sent relative to target_frame"); + } + if (client.call(plan_route)) { if (plan_route.response.success) @@ -210,34 +227,26 @@ namespace mapviz_plugins } } + bool PlanRoutePlugin::handleMousePress(QMouseEvent* event) { selected_point_ = -1; int closest_point = 0; double closest_distance = std::numeric_limits::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 mouse_pos = event->localPos(); - QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(waypoint.x(), waypoint.y())); - - 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; @@ -346,51 +329,50 @@ namespace mapviz_plugins void PlanRoutePlugin::Draw(double x, double y, double scale) { stu::Transform transform; + bool show_server_route = ( !failed_service_ && route_preview_ ); if (tf_manager_.GetTransform(target_frame_, stu::_wgs84_frame, transform)) { - if (!failed_service_) - { - 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); - glBegin(GL_LINE_STRIP); - - for (size_t i = 0; i < route.points.size(); i++) - { - glVertex2d(route.points[i].position().x(), route.points[i].position().y()); - } + show_server_route = false; + } - glEnd(); - } + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); - PrintInfo("OK"); + glBegin(GL_LINE_STRIP); + if (show_server_route) + { + glLineWidth(2); + sru::Route route = *route_preview_; + sru::transform(route, transform, target_frame_); + for (const swri_route_util::RoutePoint& point: route.points) + { + glVertex2d(point.position().x(), point.position().y()); } - - // 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++) + PrintInfo("OK"); + } + else{ + glLineWidth(1); + for (const geometry_msgs::Pose& waypoint: waypoints_) { - tf::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0); - point = transform * point; - glVertex2d(point.x(), point.y()); + glVertex2d(waypoint.position.x, waypoint.position.y); } - glEnd(); + PrintError("Route failed to convert to wgs84"); } - else + glEnd(); + + // Draw waypoints + glPointSize(20); + glColor4f(0.0, 1.0, 1.0, 1.0); + glBegin(GL_POINTS); + + for (const geometry_msgs::Pose& waypoint: waypoints_) { - PrintError("Failed to transform."); + glVertex2d(waypoint.position.x, waypoint.position.y); } + glEnd(); } + void PlanRoutePlugin::Paint(QPainter* painter, double x, double y, double scale) { painter->save(); @@ -400,20 +382,14 @@ 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 (int 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->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::number(i + 1)); } - painter->restore(); }