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);