Fawkes API  Fawkes Development Version
gazsim_vis_localization_thread.cpp
1 /***************************************************************************
2  * gazsim_vis_localization_thread.h - Plugin visualizes the localization
3  *
4  * Created: Tue Sep 17 15:38:34 2013
5  * Copyright 2013 Frederik Zwilling
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "gazsim_vis_localization_thread.h"
22 
23 #include <aspect/logging.h>
24 #include <interfaces/Position3DInterface.h>
25 #include <tf/types.h>
26 #include <utils/math/angle.h>
27 
28 #include <cmath>
29 #include <gazebo/msgs/msgs.hh>
30 #include <gazebo/transport/Node.hh>
31 #include <gazebo/transport/transport.hh>
32 #include <stdio.h>
33 
34 using namespace fawkes;
35 using namespace gazebo;
36 
37 /** @class VisLocalizationThread "gazsim_localization_thread.h"
38  * Thread simulates the Localization in Gazebo
39  * @author Frederik Zwilling
40  */
41 
42 /** Constructor. */
44 : Thread("VisLocalizationThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
46 {
47 }
48 
49 void
51 {
52  logger->log_debug(name(), "Initializing Visualization of the Localization");
53 
54  //read config values
55  update_rate_ = config->get_float("/gazsim/visualization/localization/update-rate");
56  robot_name_ = config->get_string("/gazsim/robot-name");
57  label_script_name_ = config->get_string("/gazsim/visualization/label-script-name");
58  arrow_script_name_ = config->get_string("/gazsim/visualization/label-arrow-name");
59  location_scripts_ = config->get_string("/gazsim/visualization/location-scripts");
60  location_textures_ = config->get_string("/gazsim/visualization/location-textures");
61  parent_name_ = config->get_string("/gazsim/visualization/localization/parent-name");
62  label_size_ = config->get_float("/gazsim/visualization/localization/label-size");
63  label_height_ = config->get_float("/gazsim/visualization/localization/label-height");
64 
65  last_update_time_ = clock->now().in_sec();
66 
67  //open interface
68  pose_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");
69 
70  //create publisher
71  visual_publisher_ = gazebo_world_node->Advertise<gazebo::msgs::Visual>("~/visual", 5);
72 }
73 
74 void
76 {
77  blackboard->close(pose_if_);
78 }
79 
80 void
82 {
83  //visualize the estimated position of the robot every few seconds
84  fawkes::Time new_time = clock->now();
85  double time_elapsed = new_time.in_sec() - last_update_time_.in_sec();
86  if (time_elapsed > 1 / update_rate_) {
87  last_update_time_ = new_time;
88 
89  //read pose
90  pose_if_->read();
91  double x = pose_if_->translation(0);
92  double y = pose_if_->translation(1);
93  //calculate ori from quaternion in interface
94  double *quat = pose_if_->rotation();
95  double ori = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));
96  if (std::isnan(ori)) {
97  ori = 0.0;
98  }
99 
100  //create label with number
101  msgs::Visual msg_number;
102  msg_number.set_name((robot_name_ + "-localization-label").c_str());
103  msg_number.set_parent_name(parent_name_.c_str());
104  msgs::Geometry *geomMsg = msg_number.mutable_geometry();
105  geomMsg->set_type(msgs::Geometry::PLANE);
106 #if GAZEBO_MAJOR_VERSION > 5
107  msgs::Set(geomMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));
108  msgs::Set(geomMsg->mutable_plane()->mutable_size(),
109  ignition::math::Vector2d(label_size_, label_size_));
110 #else
111  msgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
112  msgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));
113  msg_number.set_transparency(0.2);
114 #endif
115  msg_number.set_cast_shadows(false);
116 #if GAZEBO_MAJOR_VERSION > 5
117  msgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));
118 #else
119  msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));
120 #endif
121  msgs::Material::Script *script = msg_number.mutable_material()->mutable_script();
122  script->add_uri(location_scripts_.c_str());
123  script->add_uri(location_textures_.c_str());
124  script->set_name(label_script_name_.c_str());
125 
126  visual_publisher_->Publish(msg_number);
127 
128  //create label with direction arrow
129 #if GAZEBO_MAJOR_VERSION <= 5
130  msgs::Visual msg_arrow;
131  msg_arrow.set_name((robot_name_ + "-localization-arrow").c_str());
132  msg_arrow.set_parent_name(parent_name_.c_str());
133  msgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();
134  geomArrowMsg->set_type(msgs::Geometry::PLANE);
135 # if GAZEBO_MAJOR_VERSION > 5
136  msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(),
137  ignition::math::Vector3d(0.0, 0.0, 1.0));
138  msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));
139 # else
140  msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
141  msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));
142  msg_arrow.set_transparency(0.4);
143 # endif
144  msg_arrow.set_cast_shadows(false);
145 # if GAZEBO_MAJOR_VERSION > 5
146  msgs::Set(msg_arrow.mutable_pose(),
147  ignition::math::Pose3d(
148  x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
149 # else
150  msgs::Set(msg_arrow.mutable_pose(),
151  math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
152 # endif
153  msgs::Material::Script *arrow_script = msg_arrow.mutable_material()->mutable_script();
154  arrow_script->add_uri(location_scripts_.c_str());
155  arrow_script->add_uri(location_textures_.c_str());
156  arrow_script->set_name(arrow_script_name_.c_str());
157 
158  visual_publisher_->Publish(msg_arrow);
159 #endif
160  }
161 }
double * rotation() const
Get rotation value.
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
virtual void init()
Initialize the thread.
Fawkes library namespace.
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
Definition: gazebo.h:51
A class for handling time.
Definition: time.h:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void loop()
Code to execute in the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Time now() const
Get the current time.
Definition: clock.cpp:242
Thread aspect to use blocked timing.
Position3DInterface Fawkes BlackBoard Interface.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
const char * name() const
Get name of thread.
Definition: thread.h:100
double * translation() const
Get translation value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.