21 #include "gazsim_vis_localization_thread.h" 23 #include <aspect/logging.h> 24 #include <interfaces/Position3DInterface.h> 26 #include <utils/math/angle.h> 29 #include <gazebo/msgs/msgs.hh> 30 #include <gazebo/transport/Node.hh> 31 #include <gazebo/transport/transport.hh> 35 using namespace gazebo;
44 :
Thread(
"VisLocalizationThread",
Thread::OPMODE_WAITFORWAKEUP),
55 update_rate_ =
config->
get_float(
"/gazsim/visualization/localization/update-rate");
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");
71 visual_publisher_ =
gazebo_world_node->Advertise<gazebo::msgs::Visual>(
"~/visual", 5);
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;
95 double ori = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));
96 if (std::isnan(ori)) {
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_));
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);
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));
119 msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));
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());
126 visual_publisher_->Publish(msg_number);
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));
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);
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 - M_PI / 2));
150 msgs::Set(msg_arrow.mutable_pose(),
151 math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - M_PI / 2));
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());
158 visual_publisher_->Publish(msg_arrow);
double * rotation() const
Get rotation value.
double in_sec() const
Convet time to seconds.
virtual void init()
Initialize the thread.
Fawkes library namespace.
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
A class for handling time.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void loop()
Code to execute in the thread.
Clock * clock
By means of this member access to the clock is given.
Time now() const
Get the current time.
Thread aspect to use blocked timing.
Position3DInterface Fawkes BlackBoard Interface.
void read()
Read from BlackBoard into local copy.
const char * name() const
Get name of thread.
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.
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.
VisLocalizationThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.