visualization.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 #include <swri_route_util/visualization.h>
00030 
00031 #include <swri_route_util/util.h>
00032 
00033 namespace vm = visualization_msgs;
00034 namespace mnm = marti_nav_msgs;
00035 
00036 namespace swri_route_util
00037 {
00038 static geometry_msgs::Point makePoint(const double x, const double y)
00039 {
00040   geometry_msgs::Point pt;
00041   pt.x = x;
00042   pt.y = y;
00043   pt.z = 0.0;
00044   return pt;
00045 }
00046 
00047 void markerForRouteSpeeds(
00048   vm::Marker &m,
00049   const Route &route,
00050   const mnm::RouteSpeedArray &speeds,
00051   double scale)
00052 {
00053   m.header.frame_id = route.header.frame_id;
00054   m.header.stamp = ros::Time::now();
00055   // m.ns = ;
00056   // m.id = ;
00057   m.type = vm::Marker::LINE_LIST;
00058   m.action = vm::Marker::ADD;
00059   m.pose.position.x = 0.0;
00060   m.pose.position.y = 0.0;
00061   m.pose.position.z = 0.0;
00062   m.pose.orientation.x = 0.0;
00063   m.pose.orientation.y = 0.0;
00064   m.pose.orientation.z = 0.0;
00065   m.pose.orientation.w = 1.0;
00066   m.scale.x = 1.0;
00067   m.scale.y = 1.0;
00068   m.scale.z = 1.0;
00069   m.color.r = 0.0;
00070   m.color.g = 0.0;
00071   m.color.b = 0.0;
00072   m.color.a = 1.0;
00073   m.lifetime = ros::Duration(0);
00074   m.frame_locked = false;
00075 
00076   m.points.reserve(speeds.speeds.size()*2);
00077 
00078   for (auto const &speed : speeds.speeds) {
00079     mnm::RoutePosition position;
00080     position.id = speed.id;
00081     position.distance = speed.distance;
00082 
00083     RoutePoint p;
00084     if (!interpolateRoutePosition(p, route, position, true)) {
00085       continue;
00086     }
00087 
00088     tf::Vector3 p1 = p.position();
00089     tf::Vector3 v = tf::Transform(p.orientation()) * tf::Vector3(0.0, 1.0, 0.0);
00090     tf::Vector3 p2 = p1 + scale*speed.speed*v;
00091 
00092     m.points.push_back(makePoint(p1.x(), p1.y()));
00093     m.points.push_back(makePoint(p2.x(), p2.y()));
00094   }
00095 }
00096 }  // namespace swri_route_util


swri_route_util
Author(s):
autogenerated on Thu Jun 6 2019 20:35:04