Skip to content

Commit 821b9f4

Browse files
MatCat960MatCat960
committed
add rviz gui for gmm + minor fixes
1 parent b2fbbf8 commit 821b9f4

File tree

5 files changed

+75
-20
lines changed

5 files changed

+75
-20
lines changed

‎launch/distri_gmm_calculator.launch.py‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,14 +22,14 @@
2222
def generate_launch_description():
2323
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
2424
ID = 0
25-
ROBOTS_NUM = 8
26-
NUM_SAMPLES = 500
25+
ROBOTS_NUM = 5
26+
NUM_SAMPLES = 300
2727
TARGETS_NUM = 4
2828
SENS_RANGE = 5.0
2929
COMM_RANGE = 7.0
3030
NOISE_DEV = 0.5
3131
ENV_SIZE = 20.0
32-
GRAPHICS_ON = False
32+
GRAPHICS_ON = True
3333

3434
GUI = False
3535

@@ -47,11 +47,15 @@ def generate_launch_description():
4747
{"COMM_RANGE": COMM_RANGE},
4848
{"NOISE_DEV": NOISE_DEV},
4949
{"ENV_SIZE": ENV_SIZE},
50+
# {"GUI", GUI},
5051
{"GRAPHICS_ON": GRAPHICS_ON}],
5152
output='screen')
53+
54+
GRAPHICS_ON = False
5255

5356
launch_list.append(n)
5457

58+
5559
if GUI:
5660
pkg_path = get_package_prefix('gmm_coverage')
5761
config_path = os.path.join(pkg_path, '..', '..', 'src', 'gmm_coverage')
@@ -70,6 +74,7 @@ def generate_launch_description():
7074
package='gmm_coverage',
7175
node_executable='gmm_visualizer',
7276
name='gmm_visualizer',
77+
remappings=[('/gaussian_mixture_model', '/gaussian_mixture_model_0')],
7378
output='screen'
7479
)
7580
launch_list.append(gmm_visualizer)

‎launch/distributed_gmm.launch.py‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818

1919
def generate_launch_description():
2020
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
21-
ROBOTS_NUM = 8
21+
ROBOTS_NUM = 5
2222
ROBOT_RANGE = 10.0
2323
AREA_SIZE_x = 20.0
2424
AREA_SIZE_y = 20.0
@@ -57,6 +57,7 @@ def generate_launch_description():
5757
package='gmm_coverage',
5858
node_executable='gmm_visualizer',
5959
name='gmm_visualizer',
60+
remappings=[('/gaussian_mixture_model', '/gaussian_mixture_model_0')],
6061
output='screen'
6162
)
6263
ld.append(gmm_visualizer)

‎launch/gmm_online_world.launch.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
1616

1717
AREA_W = 10.0
18-
ROBOTS_NUM = 8
18+
ROBOTS_NUM = 5
1919
TARGETS_NUM = 4
2020

2121

@@ -37,8 +37,8 @@ def generate_launch_description():
3737
theta = []
3838

3939
for i in range(ROBOTS_NUM):
40-
ranx = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
41-
rany = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
40+
ranx = random.uniform(-0.0*AREA_W, 0.5*AREA_W)
41+
rany = random.uniform(-0.0*AREA_W, 0.5*AREA_W)
4242
th = random.uniform(0,6)
4343

4444
x_pos.append(ranx)
@@ -61,8 +61,8 @@ def generate_launch_description():
6161
xtargets = []
6262
ytargets = []
6363
for i in range(TARGETS_NUM):
64-
xt = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
65-
yt = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
64+
xt = random.uniform(-0.0*AREA_W, 0.5*AREA_W)
65+
yt = random.uniform(-0.0*AREA_W, 0.5*AREA_W)
6666
xtargets.append(xt)
6767
ytargets.append(yt)
6868
target = Node(package='gazebo_ros', executable='spawn_entity.py',
@@ -77,7 +77,7 @@ def generate_launch_description():
7777
target_pub = Node(
7878
package='gmm_coverage',
7979
node_executable='target_publisher_node',
80-
name='target_pub',
80+
name='target_pub'+str(i),
8181
parameters=[{"XT": xtargets[i]},
8282
{"YT": ytargets[i]},
8383
{"ID": i}],

‎rviz/gmm.rviz‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -516,7 +516,7 @@ Visualization Manager:
516516
Enabled: true
517517
Name: MarkerArray
518518
Namespaces:
519-
{}
519+
"": true
520520
Topic:
521521
Depth: 5
522522
Durability Policy: Volatile
@@ -671,7 +671,7 @@ Visualization Manager:
671671
Enabled: true
672672
Global Options:
673673
Background Color: 48; 48; 48
674-
Fixed Frame: world
674+
Fixed Frame: odom
675675
Frame Rate: 30
676676
Name: root
677677
Tools:
@@ -711,25 +711,25 @@ Visualization Manager:
711711
Views:
712712
Current:
713713
Class: rviz_default_plugins/Orbit
714-
Distance: 13.117227554321289
714+
Distance: 22.395313262939453
715715
Enable Stereo Rendering:
716716
Stereo Eye Separation: 0.05999999865889549
717717
Stereo Focal Distance: 1
718718
Swap Stereo Eyes: false
719719
Value: false
720720
Focal Point:
721-
X: 0.39492735266685486
722-
Y: -1.1991099119186401
723-
Z: 0.7388123273849487
721+
X: -0.1150718554854393
722+
Y: -0.9410849809646606
723+
Z: 0.9114724397659302
724724
Focal Shape Fixed Size: true
725725
Focal Shape Size: 0.05000000074505806
726726
Invert Z Axis: false
727727
Name: Current View
728728
Near Clip Distance: 0.009999999776482582
729-
Pitch: 0.9997971653938293
729+
Pitch: 0.6047975420951843
730730
Target Frame: <Fixed Frame>
731731
Value: Orbit (rviz)
732-
Yaw: 4.733598232269287
732+
Yaw: 4.743598461151123
733733
Saved: ~
734734
Window Geometry:
735735
Displays:
@@ -745,5 +745,5 @@ Window Geometry:
745745
Views:
746746
collapsed: false
747747
Width: 1848
748-
X: 1992
748+
X: 72
749749
Y: 27

‎src/gmm_calc.cpp‎

Lines changed: 50 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@
3535
#include <nav_msgs/msg/odometry.hpp>
3636
#include "turtlebot3_msgs/msg/gaussian.hpp"
3737
#include "turtlebot3_msgs/msg/gmm.hpp"
38+
#include <visualization_msgs/msg/marker.hpp>
39+
#include <visualization_msgs/msg/marker_array.hpp>
3840

3941
#include "gaussian_mixture_model/gaussian_mixture_model.h"
4042

@@ -68,6 +70,8 @@ class Supervisor : public rclcpp::Node
6870
this->get_parameter("NOISE_STD", NOISE_STD);
6971
this->declare_parameter<double>("ENV_SIZE", 20.0);
7072
this->get_parameter("ENV_SIZE", ENV_SIZE);
73+
this->declare_parameter<bool>("GUI", false);
74+
this->get_parameter("GUI", GUI);
7175
this->declare_parameter<bool>("GRAPHICS_ON", true);
7276
this->get_parameter("GRAPHICS_ON", GRAPHICS_ON);
7377
//-----------------------------------------------------------------------------------------------------------------------------------
@@ -94,7 +98,7 @@ class Supervisor : public rclcpp::Node
9498
}
9599
}
96100
communication_pub_ = this->create_publisher<geometry_msgs::msg::Point>("robot" + std::to_string(ID) + "/communication_topic", 1);
97-
timer_ = this->create_wall_timer(2000ms, std::bind(&Supervisor::loop, this));
101+
timer_ = this->create_wall_timer(100ms, std::bind(&Supervisor::loop, this));
98102

99103
// ------------------------------------------------- Initialize GMM and samples -----------------------------------------------------
100104
// Set up random number generator
@@ -132,6 +136,13 @@ class Supervisor : public rclcpp::Node
132136
app_gui.reset(new Graphics{ENV_SIZE, ENV_SIZE, -0.5*ENV_SIZE, -0.5*ENV_SIZE, 2.0});
133137
}
134138

139+
// ------------------------------------------ GUI Publisher ------------------------
140+
if (GUI)
141+
{
142+
SHAPE = visualization_msgs::msg::Marker::SPHERE;
143+
samples_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/gmm_"+std::to_string(ID)+"_samples", 1);
144+
}
145+
135146

136147

137148

@@ -151,6 +162,7 @@ class Supervisor : public rclcpp::Node
151162
void neighCallback(const nav_msgs::msg::Odometry::SharedPtr msg, int i);
152163
void communicationCallback(const geometry_msgs::msg::Point::SharedPtr msg, int i);
153164
Eigen::VectorXd addNoise(Eigen::VectorXd point);
165+
void publishSamples();
154166

155167
private:
156168
int ID;
@@ -161,6 +173,7 @@ class Supervisor : public rclcpp::Node
161173
double SENS_RANGE;
162174
double NOISE_STD;
163175
double ENV_SIZE;
176+
bool GUI;
164177
bool GRAPHICS_ON;
165178
GaussianMixtureModel gmm;
166179
std::vector<Eigen::VectorXd> samples;
@@ -174,8 +187,12 @@ class Supervisor : public rclcpp::Node
174187
std::vector<rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr> neighbor_subs_;
175188
rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr communication_pub_;
176189
std::vector<rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr> communication_subs_;
190+
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr samples_pub_;
177191
rclcpp::TimerBase::SharedPtr timer_;
178192

193+
visualization_msgs::msg::MarkerArray marker_msg;
194+
uint32_t SHAPE;
195+
179196
//Rendering with SFML
180197
//------------------------------ graphics window -------------------------------------
181198
std::unique_ptr<Graphics> app_gui;
@@ -236,6 +253,31 @@ void Supervisor::communicationCallback(const geometry_msgs::msg::Point::SharedPt
236253
}
237254
}
238255

256+
void Supervisor::publishSamples()
257+
{
258+
for (int i = 0; i < samples.size(); i++)
259+
{
260+
visualization_msgs::msg::Marker m;
261+
m.id = i;
262+
m.header.frame_id = "odom";
263+
m.type = SHAPE;
264+
m.action = visualization_msgs::msg::Marker::ADD;
265+
m.pose.position.x = samples[i](0);
266+
m.pose.position.y = samples[i](1);
267+
m.pose.position.z = 0.0;
268+
m.scale.x = 0.05;
269+
m.scale.y = 0.05;
270+
m.scale.z = 0.05;
271+
m.color.r = 0.0f;
272+
m.color.g = 1.0f;
273+
m.color.b = 0.0f;
274+
m.color.a = 0.5; // alpha is given by its weight
275+
this->marker_msg.markers.push_back(m);
276+
}
277+
278+
this->samples_pub_->publish(this->marker_msg);
279+
}
280+
239281
Eigen::VectorXd Supervisor::addNoise(Eigen::VectorXd point)
240282
{
241283
// Generate random inputs
@@ -251,6 +293,8 @@ Eigen::VectorXd Supervisor::addNoise(Eigen::VectorXd point)
251293
return noisy_point;
252294
}
253295

296+
297+
254298
void Supervisor::loop()
255299
{
256300
auto timerstart = std::chrono::high_resolution_clock::now();
@@ -298,6 +342,11 @@ void Supervisor::loop()
298342

299343
this->gmm_pub_->publish(gmm_msg);
300344

345+
if (GUI)
346+
{
347+
publishSamples();
348+
}
349+
301350
if ((GRAPHICS_ON) && (this->app_gui->isOpen()))
302351
{
303352
this->app_gui->clear();

0 commit comments

Comments
 (0)