Skip to content

Commit

Permalink
Merge pull request #106 from nobleo/fix/38-move-visualization
Browse files Browse the repository at this point in the history 
Moved visualization.
  • Loading branch information
@lewie-donckers
lewie-donckers committed Mar 3, 2022
2 parents 0601627 + dd16487 commit ff653a6
Show file tree
Hide file tree
Showing 4 changed files with 130 additions and 101 deletions.
1 change: 0 additions & 1 deletion include/path_tracking_pid/path_tracking_pid_local_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,6 @@ class TrackingPidLocalPlanner : public nav_core::BaseLocalPlanner,
// Rviz visualization
std::unique_ptr<Visualization> visualization_;
ros::Publisher path_pub_;
ros::Publisher collision_marker_pub_;

ros::Subscriber sub_odom_;
ros::Publisher feedback_pub_;
Expand Down
19 changes: 14 additions & 5 deletions include/path_tracking_pid/visualization.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,24 +7,33 @@

namespace path_tracking_pid
{

class Visualization
{
public:
Visualization(ros::NodeHandle nh);
using point_t = geometry_msgs::Point;
using points_t = std::vector<point_t>;

explicit Visualization(ros::NodeHandle nh);

void publishControlPoint(const std_msgs::Header & header, const tf2::Transform & pose);
void publishAxlePoint(const std_msgs::Header & header, const tf2::Transform & pose);
void publishGoalPoint(const std_msgs::Header & header, const tf2::Transform & pose);
void publishPlanPoint(const std_msgs::Header & header, const tf2::Transform & pose);
void publishCollision(
const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint,
const points_t & hull, const points_t & steps, const points_t & path);

private:
ros::Publisher marker_pub_;
ros::Publisher collision_marker_pub_;

void publishSphere(
const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose,
const std_msgs::ColorRGBA & color);
void publishSphere(
const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose,
const std_msgs::ColorRGBA & color);

ros::Publisher marker_pub_;
const std_msgs::Header & header, const std::string & ns, int id,
const geometry_msgs::Pose & pose, const std_msgs::ColorRGBA & color);
};

} // namespace path_tracking_pid
107 changes: 14 additions & 93 deletions src/path_tracking_pid_local_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,6 @@ void TrackingPidLocalPlanner::initialize(
nh.param<bool>("use_tricycle_model", use_tricycle_model_, false);
nh.param<std::string>("steered_wheel_frame", steered_wheel_frame_, "steer");

collision_marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("collision_markers", 3);
visualization_ = std::make_unique<Visualization>(nh);
debug_pub_ = nh.advertise<path_tracking_pid::PidDebug>("debug", 1);
path_pub_ = nh.advertise<nav_msgs::Path>("visualization_path", 1, true);
Expand Down Expand Up @@ -298,72 +297,6 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist & cmd

uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
{
// configure rviz visualization
visualization_msgs::Marker mkSteps;
mkSteps.header.frame_id = map_frame_;
mkSteps.header.stamp = ros::Time::now();
mkSteps.ns = "extrapolated poses";
mkSteps.action = visualization_msgs::Marker::ADD;
mkSteps.pose.orientation.w = 1.0;
mkSteps.id = __COUNTER__;
mkSteps.type = visualization_msgs::Marker::POINTS;
mkSteps.scale.x = 0.5;
mkSteps.scale.y = 0.5;
mkSteps.color.r = 1.0;
mkSteps.color.g = 0.5;
mkSteps.color.a = 1.0;

visualization_msgs::Marker mkPosesOnPath;
mkPosesOnPath.header.frame_id = map_frame_;
mkPosesOnPath.header.stamp = ros::Time::now();
mkPosesOnPath.ns = "goal poses on path";
mkPosesOnPath.action = visualization_msgs::Marker::ADD;
mkPosesOnPath.pose.orientation.w = 1.0;
mkPosesOnPath.id = __COUNTER__;
mkPosesOnPath.type = visualization_msgs::Marker::POINTS;
mkPosesOnPath.scale.x = 0.5;
mkPosesOnPath.scale.y = 0.5;
mkPosesOnPath.color.r = 1.0;
mkPosesOnPath.color.g = 1.0;
mkPosesOnPath.color.a = 1.0;

visualization_msgs::Marker mkCollisionFootprint;
mkCollisionFootprint.header.frame_id = map_frame_;
mkCollisionFootprint.header.stamp = ros::Time::now();
mkCollisionFootprint.ns = "Collision footprint";
mkCollisionFootprint.action = visualization_msgs::Marker::ADD;
mkCollisionFootprint.pose.orientation.w = 1.0;
mkCollisionFootprint.id = __COUNTER__;
mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST;
mkCollisionFootprint.scale.x = 0.1;
mkCollisionFootprint.color.b = 1.0;
mkCollisionFootprint.color.a = 0.3;

visualization_msgs::Marker mkCollisionHull;
mkCollisionHull.header.frame_id = map_frame_;
mkCollisionHull.header.stamp = ros::Time::now();
mkCollisionHull.ns = "Collision polygon";
mkCollisionHull.action = visualization_msgs::Marker::ADD;
mkCollisionHull.pose.orientation.w = 1.0;
mkCollisionHull.id = __COUNTER__;
mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP;
mkCollisionHull.scale.x = 0.2;
mkCollisionHull.color.r = 1.0;
mkCollisionHull.color.a = 0.3;

visualization_msgs::Marker mkCollisionIndicator;
mkCollisionIndicator.header.frame_id = map_frame_;
mkCollisionIndicator.header.stamp = ros::Time::now();
mkCollisionIndicator.ns = "Collision object";
mkCollisionIndicator.pose.orientation.w = 1.0;
mkCollisionIndicator.id = __COUNTER__;
mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER;
mkCollisionIndicator.scale.x = 0.5;
mkCollisionIndicator.scale.y = 0.5;
mkCollisionIndicator.color.r = 1.0;
mkCollisionIndicator.color.a = 0.0;
visualization_msgs::MarkerArray mkCollision;

// Check how far we should check forward
double x_vel = pid_controller_.getControllerState().current_x_vel;
double collision_look_ahead_distance =
Expand All @@ -383,6 +316,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
auto projected_controller_state = pid_controller_.getControllerState();

// Step until lookahead is reached, for every step project the pose back to the path
std::vector<geometry_msgs::Point> step_points;
std::vector<geometry_msgs::Point> poses_on_path_points;
std::vector<tf2::Transform> projected_steps_tf;
auto projected_step_tf = tf2_convert<tf2::Transform>(tfCurPoseStamped_.transform);
projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link
Expand All @@ -398,12 +333,13 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
// Fill markers:
geometry_msgs::Point mkStep;
tf2::toMsg(next_straight_step_tf.getOrigin(), mkStep);
mkSteps.points.push_back(mkStep);
step_points.push_back(mkStep);
geometry_msgs::Point mkPointOnPath;
tf2::toMsg(projected_step_tf.getOrigin(), mkPointOnPath);
mkPosesOnPath.points.push_back(mkPointOnPath);
poses_on_path_points.push_back(mkPointOnPath);
}

std::vector<geometry_msgs::Point> collision_footprint_points;
polygon_t previous_footprint_xy;
polygon_t collision_polygon;
for (const auto & projection_tf : projected_steps_tf) {
Expand All @@ -430,8 +366,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
// Add footprint to marker
geometry_msgs::Point previous_point = footprint.back();
for (const auto & point : footprint) {
mkCollisionFootprint.points.push_back(previous_point);
mkCollisionFootprint.points.push_back(point);
collision_footprint_points.push_back(previous_point);
collision_footprint_points.push_back(point);
previous_point = point;
}
}
Expand All @@ -456,6 +392,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
costmap2d->convexFillCells(collision_polygon_hull_map, cells_in_polygon);

// Get the max cost inside the concave polygon
geometry_msgs::Point collision_point;
uint8_t max_cost = 0.0;
for (const auto & cell_in_polygon : cells_in_polygon) {
// Cost checker is cheaper than polygon checker, so lets do that first
Expand All @@ -465,41 +402,25 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
geometry_msgs::Point point;
costmap2d->mapToWorld(cell_in_polygon.x, cell_in_polygon.y, point.x, point.y);
if (boost::geometry::within(point, collision_polygon)) {
// Protip: uncomment below and 'if (cell_cost > max_cost)' to see evaluated cells
// boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(costmap2d->getMutex()));
// costmap2d->setCost(cell_in_polygon.x, cell_in_polygon.y, 100);

max_cost = cell_cost;
// Set collision indicator on suspected cell with current cost
mkCollisionIndicator.scale.z = cell_cost / 255.0;
mkCollisionIndicator.color.a = cell_cost / 255.0;
point.z = mkCollisionIndicator.scale.z * 0.5;
mkCollisionIndicator.pose.position = point;
collision_point = point;
if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
break; // Collision detected, no need to evaluate further
}
}
}
}
if (mkCollisionIndicator.scale.z > std::numeric_limits<float>::epsilon()) {
mkCollisionIndicator.action = visualization_msgs::Marker::ADD;
} else {
mkCollisionIndicator.action = visualization_msgs::Marker::DELETE;
}
mkCollision.markers.push_back(mkCollisionIndicator);

// Fiddle the polygon into a marker message
std::vector<geometry_msgs::Point> collision_hull_points;
for (const geometry_msgs::Point point : collision_polygon) {
mkCollisionHull.points.push_back(point);
collision_hull_points.push_back(point);
}

mkCollision.markers.push_back(mkCollisionFootprint);
mkCollision.markers.push_back(mkCollisionHull);
if (n_steps > 0) {
mkCollision.markers.push_back(mkSteps);
mkCollision.markers.push_back(mkPosesOnPath);
}
collision_marker_pub_.publish(mkCollision);
visualization_->publishCollision(
map_frame_, max_cost, collision_point, collision_footprint_points, collision_hull_points,
step_points, poses_on_path_points);

return max_cost;
}
Expand Down
104 changes: 102 additions & 2 deletions src/visualization.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,18 @@
#include <ros/node_handle.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <limits>
#include <path_tracking_pid/visualization.hpp>
#include <string>

namespace path_tracking_pid
{
Visualization::Visualization(ros::NodeHandle nh)
: marker_pub_{nh.advertise<visualization_msgs::Marker>("visualization_marker", 4)},
collision_marker_pub_{nh.advertise<visualization_msgs::MarkerArray>("collision_markers", 3)}
{
marker_pub_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 4);
}

void Visualization::publishControlPoint(
Expand Down Expand Up @@ -47,6 +50,103 @@ void Visualization::publishPlanPoint(const std_msgs::Header & header, const tf2:
publishSphere(header, "plan point", __COUNTER__, pose, color);
}

void Visualization::publishCollision(
const std::string & frame_id, uint8_t cost, const point_t & point, const points_t & footprint,
const points_t & hull, const points_t & steps, const points_t & path)
{
visualization_msgs::Marker mkSteps;
mkSteps.header.frame_id = frame_id;
mkSteps.header.stamp = ros::Time::now();
mkSteps.ns = "extrapolated poses";
mkSteps.action = visualization_msgs::Marker::ADD;
mkSteps.pose.orientation.w = 1.0;
mkSteps.id = __COUNTER__;
mkSteps.type = visualization_msgs::Marker::POINTS;
mkSteps.scale.x = 0.5;
mkSteps.scale.y = 0.5;
mkSteps.color.r = 1.0;
mkSteps.color.g = 0.5;
mkSteps.color.a = 1.0;
mkSteps.points = steps;

visualization_msgs::Marker mkPosesOnPath;
mkPosesOnPath.header.frame_id = frame_id;
mkPosesOnPath.header.stamp = ros::Time::now();
mkPosesOnPath.ns = "goal poses on path";
mkPosesOnPath.action = visualization_msgs::Marker::ADD;
mkPosesOnPath.pose.orientation.w = 1.0;
mkPosesOnPath.id = __COUNTER__;
mkPosesOnPath.type = visualization_msgs::Marker::POINTS;
mkPosesOnPath.scale.x = 0.5;
mkPosesOnPath.scale.y = 0.5;
mkPosesOnPath.color.r = 1.0;
mkPosesOnPath.color.g = 1.0;
mkPosesOnPath.color.a = 1.0;
mkPosesOnPath.points = path;

visualization_msgs::Marker mkCollisionFootprint;
mkCollisionFootprint.header.frame_id = frame_id;
mkCollisionFootprint.header.stamp = ros::Time::now();
mkCollisionFootprint.ns = "Collision footprint";
mkCollisionFootprint.action = visualization_msgs::Marker::ADD;
mkCollisionFootprint.pose.orientation.w = 1.0;
mkCollisionFootprint.id = __COUNTER__;
mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST;
mkCollisionFootprint.scale.x = 0.1;
mkCollisionFootprint.color.b = 1.0;
mkCollisionFootprint.color.a = 0.3;
mkCollisionFootprint.points = footprint;

visualization_msgs::Marker mkCollisionHull;
mkCollisionHull.header.frame_id = frame_id;
mkCollisionHull.header.stamp = ros::Time::now();
mkCollisionHull.ns = "Collision polygon";
mkCollisionHull.action = visualization_msgs::Marker::ADD;
mkCollisionHull.pose.orientation.w = 1.0;
mkCollisionHull.id = __COUNTER__;
mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP;
mkCollisionHull.scale.x = 0.2;
mkCollisionHull.color.r = 1.0;
mkCollisionHull.color.a = 0.3;
mkCollisionHull.points = hull;

visualization_msgs::Marker mkCollisionIndicator;
mkCollisionIndicator.header.frame_id = frame_id;
mkCollisionIndicator.header.stamp = ros::Time::now();
mkCollisionIndicator.ns = "Collision object";
mkCollisionIndicator.pose.orientation.w = 1.0;
mkCollisionIndicator.id = __COUNTER__;
mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER;
mkCollisionIndicator.scale.x = 0.5;
mkCollisionIndicator.scale.y = 0.5;
mkCollisionIndicator.color.r = 1.0;
mkCollisionIndicator.color.a = 0.0;

visualization_msgs::MarkerArray mkCollision;
mkCollisionIndicator.scale.z = cost / 255.0;
mkCollisionIndicator.color.a = cost / 255.0;
mkCollisionIndicator.pose.position = point;
mkCollisionIndicator.pose.position.z = mkCollisionIndicator.scale.z * 0.5;
if (mkCollisionIndicator.scale.z > std::numeric_limits<float>::epsilon()) {
mkCollisionIndicator.action = visualization_msgs::Marker::ADD;
} else {
mkCollisionIndicator.action = visualization_msgs::Marker::DELETE;
}

mkCollision.markers.push_back(mkCollisionIndicator);

mkCollision.markers.push_back(mkCollisionFootprint);
mkCollision.markers.push_back(mkCollisionHull);
if (!mkSteps.points.empty()) {
mkCollision.markers.push_back(mkSteps);
}
if (!mkPosesOnPath.points.empty()) {
mkCollision.markers.push_back(mkPosesOnPath);
}

collision_marker_pub_.publish(mkCollision);
}

void Visualization::publishSphere(
const std_msgs::Header & header, const std::string & ns, int id, const tf2::Transform & pose,
const std_msgs::ColorRGBA & color)
Expand All @@ -57,7 +157,7 @@ void Visualization::publishSphere(
}

void Visualization::publishSphere(
const std_msgs::Header & header, const std::string & ns, int id, geometry_msgs::Pose pose,
const std_msgs::Header & header, const std::string & ns, int id, const geometry_msgs::Pose & pose,
const std_msgs::ColorRGBA & color)
{
visualization_msgs::Marker marker;
Expand Down

0 comments on commit ff653a6

Please sign in to comment.