Skip to content

Commit

Permalink
Merge pull request #1451 from tier4/fix/sonor-cloud-issue-7
Browse files Browse the repository at this point in the history 
Fix/sonor cloud issue 7
  • Loading branch information
@hakuturu583
hakuturu583 authored Nov 21, 2024
2 parents 62fb007 + 5d9acf9 commit 8b94804
Show file tree
Hide file tree
Showing 6 changed files with 33 additions and 34 deletions.
4 changes: 2 additions & 2 deletions common/math/geometry/include/geometry/intersection/collision.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions common/math/geometry/src/intersection/collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)) >
Expand Down
4 changes: 2 additions & 2 deletions common/math/geometry/src/polygon/line_segment.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
6 changes: 3 additions & 3 deletions common/math/geometry/src/spline/catmull_rom_spline.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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) {
Expand All @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion ...isualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
47 changes: 23 additions & 24 deletions simulation/traffic_simulator/src/visualization/visualization_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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;
}
Expand Down

0 comments on commit 8b94804

Please sign in to comment.