-
Notifications
You must be signed in to change notification settings - Fork 16
/
toggle_floors.cpp
139 lines (121 loc) · 3.79 KB
/
toggle_floors.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#include <functional>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gui/GuiPlugin.hh>
#ifndef Q_MOC_RUN
#include <gazebo/transport/transport.hh>
#include <gazebo/rendering/rendering.hh>
#endif
#include <gazebo/msgs/msgs.hh>
#include <rclcpp/rclcpp.hpp>
#include <gazebo_ros/node.hpp>
#include <rmf_fleet_msgs/msg/fleet_state.hpp>
#include <rmf_fleet_msgs/msg/robot_state.hpp>
#include <string>
#include <unordered_map>
using std::string;
using FleetState = rmf_fleet_msgs::msg::FleetState;
using RobotState = rmf_fleet_msgs::msg::RobotState;
class ToggleFloors : public gazebo::GUIPlugin
{
Q_OBJECT
gazebo::transport::NodePtr node;
gazebo::transport::PublisherPtr visual_pub;
std::unordered_map<string, std::atomic<bool>> floor_visibility;
gazebo_ros::Node::SharedPtr ros_node;
rclcpp::Subscription<FleetState>::SharedPtr fleet_state_sub;
public:
ToggleFloors()
: GUIPlugin()
{
printf("ToggleFloors::ToggleFloors()\n");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init();
visual_pub = node->Advertise<gazebo::msgs::Visual>("~/visual");
}
virtual ~ToggleFloors()
{
}
void Load(sdf::ElementPtr sdf)
{
printf("ToggleFloors::Load()\n");
ros_node = gazebo_ros::Node::Get(sdf);
// toggle non-static robots
fleet_state_sub = ros_node->create_subscription<FleetState>(
"/fleet_states", rclcpp::SystemDefaultsQoS(),
[&](FleetState::UniquePtr msg)
{
bool visible;
gazebo::msgs::Visual visual_msg;
visual_msg.set_parent_name("world");
for (const RobotState& robot : msg->robots)
{
visible = floor_visibility[robot.location.level_name];
visual_msg.set_name(robot.name);
visual_msg.set_visible(visible);
visual_pub->Publish(visual_msg);
}
});
QHBoxLayout* hbox = new QHBoxLayout;
for (sdf::ElementPtr floor_ele = sdf->GetFirstElement();
floor_ele;
floor_ele = floor_ele->GetNextElement("floor"))
{
if (floor_ele->GetName() != string("floor"))
continue;
string floor_name = floor_ele->GetAttribute("name")->GetAsString();
string model_name =
floor_ele->GetAttribute("model_name")->GetAsString();
std::vector<string> models;
auto model_ele = floor_ele->GetElement("model");
while (model_ele)
{
if (model_ele->HasAttribute("name"))
models.push_back(model_ele->GetAttribute("name")->GetAsString());
model_ele = model_ele->GetNextElement("model");
}
floor_visibility[floor_name] = true;
printf(
"ToggleFloors::Load found a floor element: [%s]->[%s]\n",
floor_name.c_str(),
model_name.c_str());
QPushButton* button =
new QPushButton(QString::fromStdString(floor_name));
button->setCheckable(true);
button->setChecked(true);
connect(
button,
&QAbstractButton::clicked,
[this, button, floor_name, model_name, models]()
{
this->button_clicked(button, floor_name, model_name, models);
});
hbox->addWidget(button);
}
setLayout(hbox);
}
void button_clicked(
QPushButton* button,
string floor_name,
string model_name,
std::vector<string> models)
{
bool visible = button->isChecked();
floor_visibility[floor_name] = visible;
printf(
"clicked: [%s] %s\n",
model_name.c_str(),
visible ? "SHOW" : "HIDE");
gazebo::msgs::Visual visual_msg;
visual_msg.set_parent_name("world");
visual_msg.set_name(model_name);
visual_msg.set_visible(visible);
visual_pub->Publish(visual_msg);
for (const string& model : models)
{
visual_msg.set_name(model);
visual_pub->Publish(visual_msg);
}
}
};
#include "toggle_floors.moc"
GZ_REGISTER_GUI_PLUGIN(ToggleFloors)