Add wall_resolution param wall published once

pedsim_simulator/scenarios/tb3_house_demo_crowd.xml

@@ -2,19 +2,19 @@
 <!--This scenario file was created by SGDiCoP on 2014-04-16T15:14:48-->
 <scenario>
     <!--Obstacles-->
-  <obstacle x1="0.0" y1="-0.5" x2="-4.5" y2="-0.5"/>
-  <obstacle x1="-5.0" y1="2.0" x2="-5.0" y2="-3.0"/>
-  <obstacle x1="-5.0" y1="-4.0" x2="-7.0" y2="-4.0"/>
-  <obstacle x1="-7.5" y1="-4.0" x2="-7.5" y2="5.0"/>
-  <obstacle x1="-7.5" y1="5.0" x2="7.0" y2="5.0"/>
-  <obstacle x1="-7.5" y1="5.0" x2="7.0" y2="5.0"/>
+  <obstacle x1="0.0" y1="-0.2" x2="-4.8" y2="-0.2"/>
+  <obstacle x1="-5.0" y1="2.0" x2="-5.0" y2="-3.6"/>
+  <obstacle x1="-5.0" y1="-4.0" x2="-7.4" y2="-4.0"/>
+  <obstacle x1="-7.5" y1="-4.0" x2="-7.5" y2="4.8"/>
+  <obstacle x1="-7.5" y1="5.2" x2="7.2" y2="5.2"/>
+  <obstacle x1="-7.5" y1="5.2" x2="7.2" y2="5.2"/>
   <obstacle x1="7.5" y1="5.0" x2="7.5" y2="-5.0"/>
   <obstacle x1="7.5" y1="-5.0" x2="5.0" y2="-5.0"/>
-  <obstacle x1="5.0" y1="-5.0" x2="5.0" y2="-1.0"/>
-  <obstacle x1="5.0" y1="0.0" x2="2.0" y2="0.0"/>
-  <obstacle x1="2.0" y1="5.0" x2="2.0" y2="2.0"/>
-  <obstacle x1="0.0" y1="5.0" x2="0.0" y2="2.0"/>
-  <obstacle x1="2.0" y1="2.0" x2="0.0" y2="2.0"/>
+  <obstacle x1="5.0" y1="-5.0" x2="5.0" y2="-0.2"/>
+  <obstacle x1="5.6" y1="-0.2" x2="1.6" y2="-0.2"/>
+  <obstacle x1="2.4" y1="4.0" x2="2.4" y2="1.0"/>
+  <obstacle x1="0.0" y1="5.0" x2="0.0" y2="1.4"/>
+  <obstacle x1="2.0" y1="1.4" x2="0.0" y2="1.4"/>
 
 
   <!--Waypoints (incl. WaitingQueues)-->

pedsim_simulator/scenarios/turtlebot3_house.xml

@@ -2,19 +2,19 @@
 <!--This scenario file was created by SGDiCoP on 2014-04-16T15:14:48-->
 <scenario>
     <!--Obstacles-->
-  <obstacle x1="0.0" y1="-0.5" x2="-4.5" y2="-0.5"/>
-  <obstacle x1="-5.0" y1="2.0" x2="-5.0" y2="-3.0"/>
-  <obstacle x1="-5.0" y1="-4.0" x2="-7.0" y2="-4.0"/>
-  <obstacle x1="-7.5" y1="-4.0" x2="-7.5" y2="5.0"/>
-  <obstacle x1="-7.5" y1="5.0" x2="7.0" y2="5.0"/>
-  <obstacle x1="-7.5" y1="5.0" x2="7.0" y2="5.0"/>
+  <obstacle x1="0.0" y1="-0.2" x2="-4.8" y2="-0.2"/>
+  <obstacle x1="-5.0" y1="2.0" x2="-5.0" y2="-3.6"/>
+  <obstacle x1="-5.0" y1="-4.0" x2="-7.4" y2="-4.0"/>
+  <obstacle x1="-7.5" y1="-4.0" x2="-7.5" y2="4.8"/>
+  <obstacle x1="-7.5" y1="5.2" x2="7.2" y2="5.2"/>
+  <obstacle x1="-7.5" y1="5.2" x2="7.2" y2="5.2"/>
   <obstacle x1="7.5" y1="5.0" x2="7.5" y2="-5.0"/>
   <obstacle x1="7.5" y1="-5.0" x2="5.0" y2="-5.0"/>
-  <obstacle x1="5.0" y1="-5.0" x2="5.0" y2="-1.0"/>
-  <obstacle x1="5.0" y1="0.0" x2="2.0" y2="0.0"/>
-  <obstacle x1="2.0" y1="5.0" x2="2.0" y2="2.0"/>
-  <obstacle x1="0.0" y1="5.0" x2="0.0" y2="2.0"/>
-  <obstacle x1="2.0" y1="2.0" x2="0.0" y2="2.0"/>
+  <obstacle x1="5.0" y1="-5.0" x2="5.0" y2="-0.2"/>
+  <obstacle x1="5.6" y1="-0.2" x2="1.6" y2="-0.2"/>
+  <obstacle x1="2.4" y1="4.0" x2="2.4" y2="1.0"/>
+  <obstacle x1="0.0" y1="5.0" x2="0.0" y2="1.4"/>
+  <obstacle x1="2.0" y1="1.4" x2="0.0" y2="1.4"/>
 
 
   <!--Waypoints (incl. WaitingQueues)-->

pedsim_utils/include/pedsim_utils/geometry.hpp

@@ -21,6 +21,7 @@ Quaternion poseFrom2DVelocity(const double vx, const double vy);
 std::vector<std::pair<float, float>> LineObstacleToCells(const float x1,
                                                          const float y1,
                                                          const float x2,
-                                                         const float y2);
+                                                         const float y2,
+                                                         const float resolution);
 
 }  // namespace pedsim

pedsim_utils/src/pedsim_utils/geometry.cpp

@@ -48,19 +48,20 @@ Quaternion poseFrom2DVelocity(const double vx, const double vy) {
 std::vector<std::pair<float, float>> LineObstacleToCells(const float x1,
                                                          const float y1,
                                                          const float x2,
-                                                         const float y2) {
-  int i;             // loop counter
-  int ystep, xstep;  // the step on y and x axis
-  int error;         // the error accumulated during the increment
-  int errorprev;     // *vision the previous value of the error variable
+                                                         const float y2,
+                                                         const float resolution) {
+  float i;             // loop counter
+  float ystep, xstep;  // the step on y and x axis
+  float error;         // the error accumulated during the increment
+  float errorprev;     // *vision the previous value of the error variable
   // int y = y1 - 0.5, x = x1 - 0.5;  // the line points
-  int y = y1, x = x1;  // the line points
-  int ddy, ddx;        // compulsory variables: the double values of dy and dx
-  int dx = x2 - x1;
-  int dy = y2 - y1;
-  double unit_x, unit_y;
-  unit_x = 1;
-  unit_y = 1;
+  float y = y1, x = x1;  // the line points
+  float ddy, ddx;        // compulsory variables: the double values of dy and dx
+  float dx = x2 - x1;
+  float dy = y2 - y1;
+  float unit_x, unit_y;
+  unit_x = resolution;
+  unit_y = resolution;
 
   if (dy < 0) {
     ystep = -unit_y;
@@ -85,7 +86,7 @@ std::vector<std::pair<float, float>> LineObstacleToCells(const float x1,
     // first octant (0 <= slope <= 1)
     // compulsory initialization (even for errorprev, needed when dx==dy)
     errorprev = error = dx;  // start in the middle of the square
-    for (i = 0; i < dx; i++) {
+    for (i = 0; i < dx; i = i + resolution) {
       // do not use the first point (already done)
       x += xstep;
       error += ddy;
@@ -113,7 +114,7 @@ std::vector<std::pair<float, float>> LineObstacleToCells(const float x1,
   } else {
     // the same as above
     errorprev = error = dy;
-    for (i = 0; i < dy; i++) {
+    for (i = 0; i < dy; i = i + resolution) {
       y += ystep;
       error += ddx;
       if (error > ddy) {

pedsim_visualizer/include/pedsim_visualizer/sim_visualizer.hpp

@@ -108,6 +108,8 @@ class SimVisualizer : public rclcpp::Node
   std::queue<pedsim_msgs::msg::LineObstacles::SharedPtr> q_obstacles_;
 
   std::string frame_id_;
+  bool walls_published_;
+  float walls_resolution_;
 };
 }  // namespace pedsim
 

pedsim_visualizer/launch/visualizer_launch.py

@@ -29,24 +29,30 @@ def generate_launch_description():
     # Set env var to print messages to stdout immediately
     SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
     frame_id = LaunchConfiguration('frame_id')
-
+    walls_resolution = LaunchConfiguration('walls_resolution')
+
     frame_id_cmd = DeclareLaunchArgument(
-        'frame_id', default_value='odom', description='Reference frame')
+        'frame_id', default_value = 'odom', description='Reference frame')
+
+    walls_resolution_cmd = DeclareLaunchArgument(
+        'walls_resolution', default_value = '0.2', description='Obstacles walls resolution')
 
     visualizer_cmd = Node(
         package='pedsim_visualizer',
         node_executable='pedsim_visualizer_node',
         node_name='pedsim_visualizer_node',
         output='screen',
         parameters=[{
-            "frame_id": frame_id
+            "frame_id": frame_id,
+            "walls_resolution": walls_resolution
         }]
     )
 
     # Create the launch description and populate
     ld = LaunchDescription()
 
     ld.add_action(frame_id_cmd)
+    ld.add_action(walls_resolution_cmd)
 
     # Declare the launch options
     ld.add_action(visualizer_cmd)

pedsim_visualizer/src/sim_visualizer.cpp

@@ -37,18 +37,20 @@ namespace pedsim {
 SimVisualizer::SimVisualizer(const std::string & name) : Node(name)  {
   declare_parameter("frame_id", rclcpp::ParameterValue("odom"));
   frame_id_ = get_parameter("frame_id").get_value<std::string>();
-
+  declare_parameter("walls_resolution", rclcpp::ParameterValue(1.0));
+  get_parameter("walls_resolution", walls_resolution_);
+  walls_published_ = false;
   setupPublishersAndSubscribers();
 }
 
 void SimVisualizer::run() {
   rclcpp::Rate loop_rate(40ms);
 
   while (rclcpp::ok()){
+    if (!walls_published_)
+      publishObstacleVisuals();
     publishAgentVisuals();
     publishGroupVisuals();
-    publishObstacleVisuals();
-
     rclcpp::spin_some(shared_from_this());
     loop_rate.sleep();
   }
@@ -148,7 +150,6 @@ void SimVisualizer::publishObstacleVisuals() {
   }
 
   const auto current_obstacles = q_obstacles_.front();
-
   visualization_msgs::msg::Marker walls_marker;
   walls_marker.header.frame_id = frame_id_;
   walls_marker.header.stamp = now();
@@ -157,24 +158,25 @@ void SimVisualizer::publishObstacleVisuals() {
   walls_marker.color.r = 0.647059;
   walls_marker.color.g = 0.164706;
   walls_marker.color.b = 0.164706;
-  walls_marker.scale.x = 1.0;
-  walls_marker.scale.y = 1.0;
+  walls_marker.scale.x = walls_resolution_;
+  walls_marker.scale.y = walls_resolution_;
   walls_marker.scale.z = 2.0;
   walls_marker.pose.position.z = walls_marker.scale.z / 2.0;
   walls_marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
 
   for (const auto& line : current_obstacles->obstacles) {
     for (const auto& cell : LineObstacleToCells(line.start.x, line.start.y,
-                                                line.end.x, line.end.y)) {
+                                                line.end.x, line.end.y,
+                                                walls_resolution_)) {
       geometry_msgs::msg::Point p;
       p.x = cell.first;
       p.y = cell.second;
       p.z = 0.0;
       walls_marker.points.push_back(p);
     }
   }
-
   pub_obstacles_visuals_->publish(walls_marker);
+  walls_published_ = true;
 }
 
 void SimVisualizer::setupPublishersAndSubscribers() {