Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

plan_route_plugin: waypoints_ relative to target_frame #554

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

plan_route_plugin: waypoints_ relative to target_frame #554

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
16c135d
plan_route_plugin: waypoints_ relative to target_frame
facontidavide Dec 13, 2017
377fa53
more c++11
facontidavide Dec 13, 2017
a644626
minor changes
facontidavide Dec 13, 2017
2d033d6
simplification
facontidavide Dec 14, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
178 changes: 77 additions & 101 deletions mapviz_plugins/src/plan_route_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,11 +126,28 @@ namespace mapviz_plugins
ros::ServiceClient client = node_.serviceClient<mnm::PlanRoute>(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<unsigned char>(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)
Expand Down Expand Up @@ -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<double>::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<int>(i);
closest_distance = distance;
closest_point = static_cast<int>(i);
}
}
}

if (event->button() == Qt::LeftButton)
Expand Down Expand Up @@ -272,17 +281,10 @@ namespace mapviz_plugins
{
if (selected_point_ >= 0 && static_cast<size_t>(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;
Expand All @@ -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;
Expand All @@ -326,18 +317,10 @@ namespace mapviz_plugins
{
if (selected_point_ >= 0 && static_cast<size_t>(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;
Expand All @@ -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();
Expand All @@ -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<std::string>(i + 1)));
}
painter->drawText(rect, Qt::AlignHCenter | Qt::AlignVCenter, QString::number(i + 1));
}

painter->restore();
}

Expand Down