21 #include "gazsim_vis_localization_thread.h" 26 #include <utils/math/angle.h> 28 #include <interfaces/Position3DInterface.h> 30 #include <gazebo/transport/Node.hh> 31 #include <gazebo/msgs/msgs.hh> 32 #include <gazebo/transport/transport.hh> 33 #include <aspect/logging.h> 45 :
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);
83 double time_elapsed = new_time.
in_sec() - last_update_time_.
in_sec();
84 if(time_elapsed > 1 / update_rate_)
86 last_update_time_ = new_time;
94 double ori = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));
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(), ignition::math::Vector2d(label_size_, label_size_));
110 msgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
111 msgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));
112 msg_number.set_transparency(0.2);
114 msg_number.set_cast_shadows(
false);
115 #if GAZEBO_MAJOR_VERSION > 5 116 msgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));
118 msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));
120 msgs::Material::Script* script = msg_number.mutable_material()->mutable_script();
121 script->add_uri(location_scripts_.c_str());
122 script->add_uri(location_textures_.c_str());
123 script->set_name(label_script_name_.c_str());
125 visual_publisher_->Publish(msg_number);
128 #if GAZEBO_MAJOR_VERSION <= 5 129 msgs::Visual msg_arrow;
130 msg_arrow.set_name((robot_name_ +
"-localization-arrow").c_str());
131 msg_arrow.set_parent_name(parent_name_.c_str());
132 msgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();
133 geomArrowMsg->set_type(msgs::Geometry::PLANE);
134 #if GAZEBO_MAJOR_VERSION > 5 135 msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));
136 msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));
138 msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
139 msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));
140 msg_arrow.set_transparency(0.4);
142 msg_arrow.set_cast_shadows(
false);
143 #if GAZEBO_MAJOR_VERSION > 5 144 msgs::Set(msg_arrow.mutable_pose(), ignition::math::Pose3d(x, y, label_height_ + 0.01, 0, 0, ori - M_PI / 2));
146 msgs::Set(msg_arrow.mutable_pose(), math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - M_PI / 2));
148 msgs::Material::Script* arrow_script = msg_arrow.mutable_material()->mutable_script();
149 arrow_script->add_uri(location_scripts_.c_str());
150 arrow_script->add_uri(location_textures_.c_str());
151 arrow_script->set_name(arrow_script_name_.c_str());
153 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.