diff --git a/config/rviz.rviz b/config/rviz.rviz index effcea2..ded7654 100644 --- a/config/rviz.rviz +++ b/config/rviz.rviz @@ -4,8 +4,8 @@ Panels: Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.7647058963775635 - Tree Height: 743 + Splitter Ratio: 0.5427135825157166 + Tree Height: 746 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -93,14 +93,13 @@ Visualization Manager: Topic: /footprint Unreliable: false Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 0; 63; 252 + - Class: rviz/MarkerArray Enabled: true - Name: PredictFootPrint - Queue Size: 10 - Topic: /local_planner/point_follow_planner/predict_footprint - Unreliable: false + Marker Topic: /local_planner/point_follow_planner/predict_footprints + Name: PredictFootprints + Namespaces: + /local_planner/point_follow_planner/predict_footprints: true + Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true @@ -166,7 +165,7 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000373fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000373000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000373fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000373000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000031f00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000019000000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000373fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000373000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ea0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/include/point_follow_planner/point_follow_planner.h b/include/point_follow_planner/point_follow_planner.h index 178f7d6..a0be431 100644 --- a/include/point_follow_planner/point_follow_planner.h +++ b/include/point_follow_planner/point_follow_planner.h @@ -92,7 +92,10 @@ class PointFollowPlanner const ros::Publisher &pub); void visualize_trajectories( const std::vector> &trajectories, const double r, const double g, const double b, - const int trajectories_size, const ros::Publisher &pub); + const ros::Publisher &pub); + void visualize_footprints( + const std::vector &trajectory, const double r, const double g, const double b, + const ros::Publisher &pub); // param double hz_; @@ -130,7 +133,7 @@ class PointFollowPlanner ros::Publisher cmd_vel_pub_; ros::Publisher candidate_trajectories_pub_; ros::Publisher best_trajectory_pub_; - ros::Publisher predict_footprint_pub_; + ros::Publisher predict_footprints_pub_; ros::Publisher finish_flag_pub_; ros::Subscriber footprint_sub_; ros::Subscriber goal_sub_; diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp index b101f7b..a2f6803 100644 --- a/src/point_follow_planner.cpp +++ b/src/point_follow_planner.cpp @@ -55,7 +55,7 @@ PointFollowPlanner::PointFollowPlanner(void) cmd_vel_pub_ = nh_.advertise("/cmd_vel", 1); best_trajectory_pub_ = private_nh_.advertise("best_trajectory", 1); candidate_trajectories_pub_ = private_nh_.advertise("candidate_trajectories", 1); - predict_footprint_pub_ = private_nh_.advertise("predict_footprint", 1); + predict_footprints_pub_ = private_nh_.advertise("predict_footprints", 1); finish_flag_pub_ = private_nh_.advertise("finish_flag", 1); footprint_sub_ = nh_.subscribe("/footprint", 1, &PointFollowPlanner::footprint_callback, this); @@ -431,9 +431,9 @@ geometry_msgs::Twist PointFollowPlanner::calc_cmd_vel() trajectories.push_back(best_traj); } - visualize_trajectory(best_traj, 1.0, 0.0, 0.0, best_trajectory_pub_); - visualize_trajectories(trajectories, 0.0, 1.0, 0.0, 1000, candidate_trajectories_pub_); - predict_footprint_pub_.publish(transform_footprint(best_traj.back())); + visualize_trajectory(best_traj, 1, 0, 0, best_trajectory_pub_); + visualize_trajectories(trajectories, 0, 1, 0, candidate_trajectories_pub_); + visualize_footprints(best_traj, 0, 0, 1, predict_footprints_pub_); return cmd_vel; } @@ -532,12 +532,10 @@ void PointFollowPlanner::visualize_trajectory( void PointFollowPlanner::visualize_trajectories( const std::vector> &trajectories, const double r, const double g, const double b, - const int trajectories_size, const ros::Publisher &pub) + const ros::Publisher &pub) { visualization_msgs::MarkerArray v_trajectories; - int count = 0; - - for (; count < trajectories.size(); count++) + for (int i = 0; i < trajectories.size(); i++) { visualization_msgs::Marker v_trajectory; v_trajectory.header.frame_id = robot_frame_; @@ -550,13 +548,13 @@ void PointFollowPlanner::visualize_trajectories( v_trajectory.type = visualization_msgs::Marker::LINE_STRIP; v_trajectory.action = visualization_msgs::Marker::ADD; v_trajectory.lifetime = ros::Duration(); - v_trajectory.id = count; + v_trajectory.id = i; v_trajectory.scale.x = 0.02; geometry_msgs::Pose pose; pose.orientation.w = 1; v_trajectory.pose = pose; geometry_msgs::Point p; - for (const auto &pose : trajectories[count]) + for (const auto &pose : trajectories[i]) { p.x = pose.x_; p.y = pose.y_; @@ -564,19 +562,43 @@ void PointFollowPlanner::visualize_trajectories( } v_trajectories.markers.push_back(v_trajectory); } + pub.publish(v_trajectories); +} - for (; count < trajectories_size; count++) +void PointFollowPlanner::visualize_footprints( + const std::vector &trajectory, const double r, const double g, const double b, const ros::Publisher &pub) +{ + visualization_msgs::MarkerArray v_footprints; + for (int i = 0; i < trajectory.size(); i++) { - visualization_msgs::Marker v_trajectory; - v_trajectory.header.frame_id = robot_frame_; - v_trajectory.header.stamp = ros::Time::now(); - v_trajectory.ns = pub.getTopic(); - v_trajectory.type = visualization_msgs::Marker::LINE_STRIP; - v_trajectory.action = visualization_msgs::Marker::DELETE; - v_trajectory.lifetime = ros::Duration(); - v_trajectory.id = count; - v_trajectories.markers.push_back(v_trajectory); + visualization_msgs::Marker v_footprint; + v_footprint.header.frame_id = robot_frame_; + v_footprint.header.stamp = ros::Time::now(); + v_footprint.color.r = r; + v_footprint.color.g = g; + v_footprint.color.b = b; + v_footprint.color.a = 0.8; + v_footprint.ns = pub.getTopic(); + v_footprint.type = visualization_msgs::Marker::LINE_STRIP; + v_footprint.action = visualization_msgs::Marker::ADD; + v_footprint.lifetime = ros::Duration(); + v_footprint.id = i; + v_footprint.scale.x = 0.01; + geometry_msgs::Pose pose; + pose.orientation.w = 1; + v_footprint.pose = pose; + geometry_msgs::Point p; + const geometry_msgs::PolygonStamped footprint = transform_footprint(trajectory[i]); + for (const auto &point : footprint.polygon.points) + { + p.x = point.x; + p.y = point.y; + v_footprint.points.push_back(p); + } + p.x = footprint.polygon.points.front().x; + p.y = footprint.polygon.points.front().y; + v_footprint.points.push_back(p); + v_footprints.markers.push_back(v_footprint); } - - pub.publish(v_trajectories); + pub.publish(v_footprints); }