Fix/sonor cloud issue 7

common/math/geometry/include/geometry/intersection/collision.hpp

@@ -26,8 +26,8 @@ namespace math
 namespace geometry
 {
 bool checkCollision2D(
-  geometry_msgs::msg::Pose pose0, traffic_simulator_msgs::msg::BoundingBox bbox0,
-  geometry_msgs::msg::Pose pose1, traffic_simulator_msgs::msg::BoundingBox bbox1);
+  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
+  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1);
 bool contains(
   const std::vector<geometry_msgs::msg::Point> & polygon, const geometry_msgs::msg::Point & point);
 }  // namespace geometry

common/math/geometry/src/intersection/collision.cpp

@@ -30,8 +30,8 @@ using boost_point = boost::geometry::model::d2::point_xy<double>;
 using boost_polygon = boost::geometry::model::polygon<boost_point>;
 
 bool checkCollision2D(
-  geometry_msgs::msg::Pose pose0, traffic_simulator_msgs::msg::BoundingBox bbox0,
-  geometry_msgs::msg::Pose pose1, traffic_simulator_msgs::msg::BoundingBox bbox1)
+  const geometry_msgs::msg::Pose & pose0, const traffic_simulator_msgs::msg::BoundingBox & bbox0,
+  const geometry_msgs::msg::Pose & pose1, const traffic_simulator_msgs::msg::BoundingBox & bbox1)
 {
   if (
     std::abs((pose0.position.z + bbox0.center.z) - (pose1.position.z + bbox1.center.z)) >

common/math/geometry/src/polygon/line_segment.cpp

@@ -314,11 +314,11 @@ auto getLineSegments(
   } else {
     std::vector<LineSegment> seg;
     for (size_t i = 0; i < points.size() - 1; i++) {
-      seg.emplace_back(LineSegment(points[i], points[i + 1]));
+      seg.emplace_back(points[i], points[i + 1]);
     }
     /// @note If true, the end point(points[points.size() - 1]) and start point(points[0]) was connected.
     if (close_start_end) {
-      seg.emplace_back(LineSegment(points[points.size() - 1], points[0]));
+      seg.emplace_back(points[points.size() - 1], points[0]);
     }
     return seg;
   }

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -36,7 +36,7 @@ auto CatmullRomSpline::getPolygon(
   std::vector<geometry_msgs::msg::Point> points;
   std::vector<geometry_msgs::msg::Point> left_bounds = getLeftBounds(width, num_points, z_offset);
   std::vector<geometry_msgs::msg::Point> right_bounds = getRightBounds(width, num_points, z_offset);
-  for (size_t i = 0; i < static_cast<size_t>(left_bounds.size() - 1); i++) {
+  for (size_t i = 0; i < left_bounds.size() - 1; i++) {
     geometry_msgs::msg::Point pr_0 = right_bounds[i];
     geometry_msgs::msg::Point pl_0 = left_bounds[i];
     geometry_msgs::msg::Point pr_1 = right_bounds[i + 1];
@@ -57,7 +57,7 @@ auto CatmullRomSpline::getRightBounds(
 {
   std::vector<geometry_msgs::msg::Point> points;
   double step_size = getLength() / static_cast<double>(num_points);
-  for (size_t i = 0; i < static_cast<size_t>(num_points + 1); i++) {
+  for (size_t i = 0; i < num_points + 1; i++) {
     double s = step_size * static_cast<double>(i);
     points.emplace_back(
       [this](const double local_width, const double local_s, const double local_z_offset) {
@@ -80,7 +80,7 @@ auto CatmullRomSpline::getLeftBounds(
 {
   std::vector<geometry_msgs::msg::Point> points;
   double step_size = getLength() / static_cast<double>(num_points);
-  for (size_t i = 0; i < static_cast<size_t>(num_points + 1); i++) {
+  for (size_t i = 0; i < num_points + 1; i++) {
     double s = step_size * static_cast<double>(i);
     points.emplace_back(
       [this](const double local_width, const double local_s, const double local_z_offset) {

rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp

@@ -64,7 +64,7 @@ Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return
 QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height)
 {
   const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock();
-  Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+  Ogre::uint8 * pDest = pixelBox.data;
   memset(pDest, 0, width * height);
   return QImage(pDest, width, height, QImage::Format_ARGB32);
 }

simulation/traffic_simulator/src/visualization/visualization_component.cpp

@@ -105,7 +105,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateDelet
   auto stamp = get_clock()->now();
   for (const auto & marker : markers_[ns].markers) {
     visualization_msgs::msg::Marker marker_msg;
-    marker_msg.action = marker_msg.DELETE;
+    marker_msg.action = visualization_msgs::msg::Marker::DELETE;
     marker_msg.header.frame_id = ns;
     marker_msg.header.stamp = stamp;
     marker_msg.ns = marker.ns;
@@ -149,7 +149,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
         goal_pose_marker.header.stamp = stamp;
         goal_pose_marker.ns = status.name;
         goal_pose_marker.id = 10 + int(goal_pose_max_size - goal_pose.size() + i);
-        goal_pose_marker.action = goal_pose_marker.ADD;
+        goal_pose_marker.action = visualization_msgs::msg::Marker::ADD;
         goal_pose_marker.type = 0;  //arrow
         goal_pose_marker.pose = goal_pose[i];
         goal_pose_marker.color = color;
@@ -160,7 +160,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
         ret.markers.emplace_back(goal_pose_marker);
 
         visualization_msgs::msg::Marker goal_pose_text_marker;
-        goal_pose_text_marker.type = goal_pose_text_marker.TEXT_VIEW_FACING;
+        goal_pose_text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
         goal_pose_text_marker.header.frame_id = "map";
         goal_pose_text_marker.header.stamp = stamp;
         goal_pose_text_marker.ns = status.name;
@@ -170,7 +170,6 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
         goal_pose_text_marker.pose.position.y = goal_pose[i].position.y;
         goal_pose_text_marker.pose.position.z = goal_pose[i].position.z + 1.0;
         goal_pose_text_marker.pose.orientation = geometry_msgs::msg::Quaternion(default_quaternion);
-        goal_pose_text_marker.type = goal_pose_text_marker.TEXT_VIEW_FACING;
         goal_pose_text_marker.scale.x = 0.0;
         goal_pose_text_marker.scale.y = 0.0;
         goal_pose_text_marker.scale.z = 0.6;
@@ -181,25 +180,25 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
         ret.markers.emplace_back(goal_pose_text_marker);
       } else {
         visualization_msgs::msg::Marker goal_pose_marker;
-        goal_pose_marker.action = goal_pose_marker.DELETE;
+        goal_pose_marker.action = visualization_msgs::msg::Marker::DELETE;
         goal_pose_marker.id = 10 + int(i - (goal_pose_max_size - goal_pose.size()));
         goal_pose_marker.ns = status.name;
         ret.markers.emplace_back(goal_pose_marker);
         visualization_msgs::msg::Marker goal_pose_text_marker;
-        goal_pose_text_marker.action = goal_pose_text_marker.DELETE;
+        goal_pose_text_marker.action = visualization_msgs::msg::Marker::ADD;
         goal_pose_text_marker.id = 100 + int(i - (goal_pose_max_size - goal_pose.size()));
         goal_pose_text_marker.ns = status.name;
         ret.markers.emplace_back(goal_pose_text_marker);
       }
     }
   } else {
     visualization_msgs::msg::Marker goal_pose_marker;
-    goal_pose_marker.action = goal_pose_marker.DELETE;
+    goal_pose_marker.action = visualization_msgs::msg::Marker::DELETE;
     goal_pose_marker.id = 10 + int(goal_pose_max_size - 1);
     goal_pose_marker.ns = status.name;
     ret.markers.emplace_back(goal_pose_marker);
     visualization_msgs::msg::Marker goal_pose_text_marker;
-    goal_pose_text_marker.action = goal_pose_text_marker.DELETE;
+    goal_pose_text_marker.action = visualization_msgs::msg::Marker::DELETE;
     goal_pose_text_marker.id = 100 + int(goal_pose_max_size - 1);
     goal_pose_text_marker.ns = status.name;
     ret.markers.emplace_back(goal_pose_text_marker);
@@ -210,8 +209,8 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   bbox.header.stamp = stamp;
   bbox.ns = status.name;
   bbox.id = 0;
-  bbox.action = bbox.ADD;
-  bbox.type = bbox.LINE_LIST;
+  bbox.action = visualization_msgs::msg::Marker::ADD;
+  bbox.type = visualization_msgs::msg::Marker::LINE_LIST;
   bbox.lifetime = rclcpp::Duration::from_seconds(0.1);
   geometry_msgs::msg::Point p0, p1, p2, p3, p4, p5, p6, p7;
 
@@ -314,14 +313,14 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   text.header.stamp = stamp;
   text.ns = status.name;
   text.id = 1;
-  text.action = text.ADD;
+  text.action = visualization_msgs::msg::Marker::ADD;
   text.pose.position.x = status.bounding_box.center.x;
   text.pose.position.y = status.bounding_box.center.y;
   text.pose.position.z =
     status.bounding_box.center.z + status.bounding_box.dimensions.z * 0.5 + 1.0;
   text.pose.position = math::geometry::transformPoint(status.pose, text.pose.position);
   text.pose.orientation = status.pose.orientation;
-  text.type = text.TEXT_VIEW_FACING;
+  text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
   text.scale.x = 0.0;
   text.scale.y = 0.0;
   text.scale.z = 0.6;
@@ -335,7 +334,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   arrow.header.stamp = stamp;
   arrow.ns = status.name;
   arrow.id = 2;
-  arrow.action = arrow.ADD;
+  arrow.action = visualization_msgs::msg::Marker::ADD;
 
   // constexpr double arrow_size = 0.3;
   double arrow_size = 0.4 * status.bounding_box.dimensions.y;
@@ -360,7 +359,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   arrow.points = {pf, pl, pr};
   arrow.colors = {color};
   arrow.pose.orientation = status.pose.orientation;
-  arrow.type = arrow.TRIANGLE_LIST;
+  arrow.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
   arrow.scale.x = 1.0;
   arrow.scale.y = 1.0;
   arrow.scale.z = 1.0;
@@ -373,11 +372,11 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
   text_action.header.stamp = stamp;
   text_action.ns = status.name;
   text_action.id = 3;
-  text_action.action = text_action.ADD;
+  text_action.action = visualization_msgs::msg::Marker::ADD;
   text_action.pose.position =
     math::geometry::transformPoint(status.pose, status.bounding_box.center);
   text_action.pose.orientation = status.pose.orientation;
-  text_action.type = text_action.TEXT_VIEW_FACING;
+  text_action.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
   text_action.scale.x = 0.0;
   text_action.scale.y = 0.0;
   text_action.scale.z = 0.4;
@@ -408,8 +407,8 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
     waypoints_marker.header.stamp = stamp;
     waypoints_marker.ns = status.name;
     waypoints_marker.id = 4;
-    waypoints_marker.action = waypoints_marker.ADD;
-    waypoints_marker.type = waypoints_marker.TRIANGLE_LIST;
+    waypoints_marker.action = visualization_msgs::msg::Marker::ADD;
+    waypoints_marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
     size_t num_points = 20;
     waypoints_marker.points = spline.getPolygon(status.bounding_box.dimensions.y, num_points);
     waypoints_marker.color = color;
@@ -429,8 +428,8 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
       obstacle_marker.header.stamp = stamp;
       obstacle_marker.ns = status.name;
       obstacle_marker.id = 5;
-      obstacle_marker.action = obstacle_marker.ADD;
-      obstacle_marker.type = obstacle_marker.CUBE;
+      obstacle_marker.action = visualization_msgs::msg::Marker::ADD;
+      obstacle_marker.type = visualization_msgs::msg::Marker::CUBE;
       obstacle_marker.pose = spline.getPose(obstacle.s);
       obstacle_marker.pose.position.z =
         obstacle_marker.pose.position.z + status.bounding_box.dimensions.z * 0.5;
@@ -441,19 +440,19 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke
       ret.markers.emplace_back(obstacle_marker);
     } else {
       visualization_msgs::msg::Marker obstacle_marker;
-      obstacle_marker.action = obstacle_marker.DELETE;
+      obstacle_marker.action = visualization_msgs::msg::Marker::DELETE;
       obstacle_marker.id = 5;
       obstacle_marker.ns = status.name;
       ret.markers.emplace_back(obstacle_marker);
     }
   } else {
     visualization_msgs::msg::Marker waypoints_marker;
-    waypoints_marker.action = waypoints_marker.DELETE;
+    waypoints_marker.action = visualization_msgs::msg::Marker::DELETE;
     waypoints_marker.id = 4;
     waypoints_marker.ns = status.name;
     ret.markers.emplace_back(waypoints_marker);
     visualization_msgs::msg::Marker obstacle_marker;
-    obstacle_marker.action = obstacle_marker.DELETE;
+    obstacle_marker.action = visualization_msgs::msg::Marker::DELETE;
     obstacle_marker.id = 5;
     obstacle_marker.ns = status.name;
     ret.markers.emplace_back(obstacle_marker);
@@ -465,7 +464,7 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateDelet
 {
   visualization_msgs::msg::MarkerArray ret;
   visualization_msgs::msg::Marker marker;
-  marker.action = marker.DELETEALL;
+  marker.action = visualization_msgs::msg::Marker::DELETEALL;
   ret.markers.emplace_back(marker);
   return ret;
 }