RouteSegmentsVisual.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Software License Agreement (BSD License) *
3  * Copyright (C) 2016 by Markus Bader <markus.bader@tuwien.ac.at> *
4  * *
5  * Redistribution and use in source and binary forms, with or without *
6  * modification, are permitted provided that the following conditions *
7  * are met: *
8  * *
9  * 1. Redistributions of source code must retain the above copyright *
10  * notice, this list of conditions and the following disclaimer. *
11  * 2. Redistributions in binary form must reproduce the above copyright *
12  * notice, this list of conditions and the following disclaimer in *
13  * the documentation and/or other materials provided with the *
14  * distribution. *
15  * 3. Neither the name of the copyright holder nor the names of its *
16  * contributors may be used to endorse or promote products derived *
17  * from this software without specific prior written permission. *
18  * *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT *
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS *
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE *
23  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, *
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, *
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
29  * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE *
30  * POSSIBILITY OF SUCH DAMAGE. *
31  ***************************************************************************/
32 
33 #include <OGRE/OgreVector3.h>
34 #include <OGRE/OgreSceneNode.h>
35 #include <OGRE/OgreSceneManager.h>
36 
38 #include <eigen3/unsupported/Eigen/Splines>
41 
42 namespace tuw_nav_rviz {
43 
44 RouteSegmentsVisual::RouteSegmentsVisual ( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ) {
45  scene_manager_ = scene_manager;
46 
47  // Ogre::SceneNode s form a tree, with each node storing the
48  // transform (position and orientation) of itself relative to its
49  // parent. Ogre does the math of combining those transforms when it
50  // is time to render.
51  //
52  // Here we create a node to store the pose of the MarkerDetection's header frame
53  // relative to the RViz fixed frame.
54  frame_node_ = parent_node->createChildSceneNode();
55 
56  // initialize global variables
57  color_start_point_ = Ogre::ColourValue ( 170, 0, 0 );
58  color_end_point_ = Ogre::ColourValue ( 0, 170, 0 );
59  color_center_point_ = Ogre::ColourValue ( 0, 0, 170 );
60  color_lines_ = Ogre::ColourValue ( 170, 0, 170 );
61  color_arcs_ = Ogre::ColourValue ( 0, 170, 170 );
62 
66 
67  scale_start_point_ = 0.05;
68  scale_end_point_ = 0.04;
69  scale_center_point_ = 0.05;
70 
71  show_acrs_ = true;
72  show_lines_ = true;
73  show_start_points_ = true;
74  show_end_points_ = false;
75  show_center_points_ = false;
76 }
77 
79  // Destroy the frame node since we don't need it anymore.
80  scene_manager_->destroySceneNode ( frame_node_ );
81 }
82 
83 void RouteSegmentsVisual::setMessage ( const tuw_nav_msgs::RouteSegments::ConstPtr& msg ) {
84  static double timeOld_;
85  if ( timeOld_ == msg->header.stamp.toSec() ) {
86  return;
87  }
88  timeOld_ = msg->header.stamp.toSec();
89 
90  startPts_.clear();
91  endPts_.clear();
92  centerPts_.clear();
93  lines_.clear ();
94  arcs_.clear ();
95  for ( size_t i = 0; i < msg->segments.size(); i++ ) {
96  const tuw::ros_msgs::RouteSegment &segment = (const tuw::ros_msgs::RouteSegment &) msg->segments[i];
97  const geometry_msgs::Pose &p0 = segment.start;
98  const geometry_msgs::Pose &pc = segment.center;
99  const geometry_msgs::Pose &p1 = segment.end;
100 
101  if ( show_start_points_ ) {
103  boost::shared_ptr<rviz::Shape> startShape = startPts_.back();
104  startShape->setColor ( color_start_point_ );
105  startShape->setPosition ( Ogre::Vector3 ( p0.position.x, p0.position.y, p0.position.z ) );
106  startShape->setOrientation ( Ogre::Quaternion ( p0.orientation.x, p0.orientation.y, p0.orientation.z, p0.orientation.w ) );
107  startShape->setScale ( Ogre::Vector3 ( scale_start_point_, scale_start_point_, scale_start_point_ ) );
108  }
109 
110  if ( show_end_points_ ) {
112  boost::shared_ptr<rviz::Shape> endShape = endPts_.back();
113  endShape->setColor ( color_end_point_ );
114  endShape->setPosition ( Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z ) );
115  endShape->setOrientation ( Ogre::Quaternion ( p1.orientation.x, p1.orientation.y, p1.orientation.z, p1.orientation.w ) );
116  endShape->setScale ( Ogre::Vector3 ( scale_end_point_, scale_end_point_, scale_end_point_ ) );
117  }
118  if ( show_center_points_ && (segment.type == tuw::ros_msgs::RouteSegment::ARC )) {
120  boost::shared_ptr<rviz::Shape> centerShape = centerPts_.back();
121  centerShape->setColor ( color_center_point_ );
122  centerShape->setPosition ( Ogre::Vector3 ( pc.position.x, pc.position.y, pc.position.z ) );
123  centerShape->setOrientation ( Ogre::Quaternion ( pc.orientation.x, pc.orientation.y, pc.orientation.z, pc.orientation.w ) );
124  centerShape->setScale ( Ogre::Vector3 ( scale_center_point_, scale_center_point_, scale_center_point_ ) );
125  }
126  if ( show_lines_ && ( segment.type == tuw::ros_msgs::RouteSegment::LINE ) ) {
129  line->setColor ( color_lines_ );
130  line->setPoints ( Ogre::Vector3 ( p0.position.x, p0.position.y, p0.position.z ), Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z ) );
131  line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
132  }
133 
134  if ( show_acrs_ && ( segment.type == tuw::ros_msgs::RouteSegment::ARC ) ) {
135  double angle_resolution = M_PI/45.;
136  std::vector<geometry_msgs::PosePtr> poses;
137 
138  segment.sample_equal_angle(poses, angle_resolution, 0);
139 
140  Ogre::Vector3 v0( p0.position.x, p0.position.y, p0.position.z);
141  Ogre::Vector3 v1;
142  for ( int i = 1; i < poses.size(); i++) {
144  boost::shared_ptr<rviz::Line> line = arcs_.back();
145  v1 = Ogre::Vector3 ( poses[i]->position.x, poses[i]->position.y, poses[i]->position.z);
146  line->setColor ( color_arcs_ );
147  line->setPoints ( v0, v1 );
148  line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
149  v0 = v1;
150  }
152  boost::shared_ptr<rviz::Line> line = arcs_.back();
153  v1 = Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z);
154  line->setColor ( color_arcs_ );
155  line->setPoints ( v0, v1 );
156  line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
157 
158  }
159 
160  }
161 
162 }
163 
164 // Position is passed through to the SceneNode.
165 void RouteSegmentsVisual::setFramePosition ( const Ogre::Vector3& position ) {
166  frame_node_->setPosition ( position );
167 }
168 
169 // Orientation is passed through to the SceneNode.
170 void RouteSegmentsVisual::setFrameOrientation ( const Ogre::Quaternion& orientation ) {
171  frame_node_->setOrientation ( orientation );
172 }
173 
174 // Color is passed through to the Shape object.
175 void RouteSegmentsVisual::setStartPointColor ( Ogre::ColourValue color ) {
176  color_start_point_ = color;
178  pnt ->setColor ( color_start_point_ );
179  }
180 }
181 // Color is passed through to the Shape object.
182 void RouteSegmentsVisual::setEndPointColor ( Ogre::ColourValue color ) {
183  color_end_point_ = color;
185  pnt ->setColor ( color_end_point_ );
186  }
187 }
188 // Color is passed through to the Shape object.
189 void RouteSegmentsVisual::setCenterPointColor ( Ogre::ColourValue color ) {
190  color_center_point_ = color;
192  pnt ->setColor ( color_center_point_ );
193  }
194 }
195 
196 
197 void RouteSegmentsVisual::setLineColor ( Ogre::ColourValue color ) {
198  color_lines_ = color;
199  for ( boost::shared_ptr <rviz::Line > &pnt : lines_ ) {
200  pnt ->setColor ( color_lines_ );
201  }
202 }
203 void RouteSegmentsVisual::setArcColor ( Ogre::ColourValue color ) {
204  color_arcs_ = color;
205  for ( boost::shared_ptr <rviz::Line > &pnt : arcs_ ) {
206  pnt ->setColor ( color_arcs_ );
207  }
208 }
209 
210 // Shape type is passed through to the Shape object.
212  shape_start_point_ = shape_type;
214  Ogre::Vector3 posOld = pnt->getPosition();
215  Ogre::Quaternion orientOld = pnt->getOrientation();
217  pnt->setColor ( color_start_point_ );
218  pnt->setPosition ( posOld );
219  pnt->setOrientation ( orientOld );
220  pnt->setScale ( Ogre::Vector3 ( scale_start_point_, scale_start_point_, scale_start_point_ ) );
221  }
222 }
223 // Shape type is passed through to the Shape object.
225  shape_end_point_ = shape_type;
227  Ogre::Vector3 posOld = pnt->getPosition();
228  Ogre::Quaternion orientOld = pnt->getOrientation();
229  pnt.reset ( new rviz::Shape ( shape_end_point_, scene_manager_, frame_node_ ) );
230  pnt->setColor ( color_end_point_ );
231  pnt->setPosition ( posOld );
232  pnt->setOrientation ( orientOld );
233  pnt->setScale ( Ogre::Vector3 ( scale_end_point_, scale_end_point_, scale_end_point_ ) );
234  }
235 }
236 // Shape type is passed through to the Shape object.
238  shape_center_point_ = shape_type;
240  Ogre::Vector3 posOld = pnt->getPosition();
241  Ogre::Quaternion orientOld = pnt->getOrientation();
243  pnt->setColor ( color_center_point_ );
244  pnt->setPosition ( posOld );
245  pnt->setOrientation ( orientOld );
246  pnt->setScale ( Ogre::Vector3 ( scale_center_point_, scale_center_point_, scale_center_point_ ) );
247  }
248 }
249 
250 // Scale is passed through to the Shape object.
252  scale_start_point_ = scale;
254  pnt ->setScale ( Ogre::Vector3 ( scale_start_point_, scale_start_point_, scale_start_point_ ) );
255  }
256 }
257 // Scale is passed through to the Shape object.
259  scale_end_point_ = scale;
261  pnt ->setScale ( Ogre::Vector3 ( scale_end_point_, scale_end_point_, scale_end_point_ ) );
262  }
263 }
264 // Scale is passed through to the Shape object.
266  scale_center_point_ = scale;
268  pnt ->setScale ( Ogre::Vector3 ( scale_center_point_, scale_center_point_, scale_center_point_ ) );
269  }
270 }
271 
272 void RouteSegmentsVisual::setShowArcs ( bool visible ) {
273  show_acrs_ = visible;
274  if ( !show_acrs_ ) arcs_.clear();
275 }
276 
277 void RouteSegmentsVisual::setShowLines ( bool visible ) {
278  show_lines_ = visible;
279  if ( !show_lines_ ) lines_.clear();
280 }
282  show_start_points_ = visible;
283  if ( !show_start_points_ ) startPts_.clear();
284 }
286  show_end_points_ = visible;
287  if ( !show_end_points_ ) endPts_.clear();
288 }
290  show_center_points_ = visible;
291  if ( !show_center_points_ ) centerPts_.clear();
292 }
293 
294 
295 } // end namespace tuw_nav_rviz
296 
static const unsigned int ARC
void setMessage(const tuw_nav_msgs::RouteSegments::ConstPtr &msg)
std::vector< boost::shared_ptr< rviz::Line > > arcs_
RouteSegmentsVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
std::vector< boost::shared_ptr< rviz::Line > > lines_
void setCenterPointColor(Ogre::ColourValue color)
static const unsigned int LINE
std::vector< boost::shared_ptr< rviz::Shape > > endPts_
void setFrameOrientation(const Ogre::Quaternion &orientation)
void setFramePosition(const Ogre::Vector3 &position)
double sample_equal_angle(std::vector< geometry_msgs::PosePtr > &poses, double angle, double distance_offset) const
void setStartPointColor(Ogre::ColourValue color)
void setArcColor(Ogre::ColourValue color)
void setEndPointShape(rviz::Shape::Type shape_type)
void setCenterPointShape(rviz::Shape::Type shape_type)
std::vector< boost::shared_ptr< rviz::Shape > > centerPts_
std::vector< boost::shared_ptr< rviz::Shape > > startPts_
void setEndPointColor(Ogre::ColourValue color)
void setLineColor(Ogre::ColourValue color)
void setStartPointShape(rviz::Shape::Type shape_type)


tuw_nav_rviz
Author(s):
autogenerated on Mon Jun 10 2019 15:40:12