visualization.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
30 
31 #include <swri_route_util/util.h>
32 
33 namespace vm = visualization_msgs;
34 namespace mnm = marti_nav_msgs;
35 
36 namespace swri_route_util
37 {
38 static geometry_msgs::Point makePoint(const double x, const double y)
39 {
40  geometry_msgs::Point pt;
41  pt.x = x;
42  pt.y = y;
43  pt.z = 0.0;
44  return pt;
45 }
46 
48  vm::Marker &m,
49  const Route &route,
50  const mnm::RouteSpeedArray &speeds,
51  double scale)
52 {
53  m.header.frame_id = route.header.frame_id;
54  m.header.stamp = ros::Time::now();
55  // m.ns = ;
56  // m.id = ;
57  m.type = vm::Marker::LINE_LIST;
58  m.action = vm::Marker::ADD;
59  m.pose.position.x = 0.0;
60  m.pose.position.y = 0.0;
61  m.pose.position.z = 0.0;
62  m.pose.orientation.x = 0.0;
63  m.pose.orientation.y = 0.0;
64  m.pose.orientation.z = 0.0;
65  m.pose.orientation.w = 1.0;
66  m.scale.x = 1.0;
67  m.scale.y = 1.0;
68  m.scale.z = 1.0;
69  m.color.r = 0.0;
70  m.color.g = 0.0;
71  m.color.b = 0.0;
72  m.color.a = 1.0;
73  m.lifetime = ros::Duration(0);
74  m.frame_locked = false;
75 
76  m.points.reserve(speeds.speeds.size()*2);
77 
78  for (auto const &speed : speeds.speeds) {
79  mnm::RoutePosition position;
80  position.id = speed.id;
81  position.distance = speed.distance;
82 
83  RoutePoint p;
84  if (!interpolateRoutePosition(p, route, position, true)) {
85  continue;
86  }
87 
88  tf::Vector3 p1 = p.position();
89  tf::Vector3 v = tf::Transform(p.orientation()) * tf::Vector3(0.0, 1.0, 0.0);
90  tf::Vector3 p2 = p1 + scale*speed.speed*v;
91 
92  m.points.push_back(makePoint(p1.x(), p1.y()));
93  m.points.push_back(makePoint(p2.x(), p2.y()));
94  }
95 }
96 } // namespace swri_route_util
static geometry_msgs::Point makePoint(const double x, const double y)
void markerForRouteSpeeds(visualization_msgs::Marker &marker, const Route &route, const marti_nav_msgs::RouteSpeedArray &speeds, double scale)
std_msgs::Header header
Definition: route.h:77
bool interpolateRoutePosition(RoutePoint &point, const Route &route, const marti_nav_msgs::RoutePosition &position, bool allow_extrapolation)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
static Time now()
const tf::Vector3 & position() const
const tf::Quaternion & orientation() const


swri_route_util
Author(s):
autogenerated on Fri Jun 7 2019 22:06:09