代码路径:moveit/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
获取规划好的路径信息:
void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
{
// Error check
if (!robot_state_ || !robot_model_)
{
ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot state or robot model loaded");
return;
}
if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg->model_id.c_str(),
robot_model_->getName().c_str());
trajectory_message_to_display_.reset();
robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
{
if (t->empty())
{
t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
}
else
{
robot_trajectory::RobotTrajectory tmp(robot_model_, "");
tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
t->append(tmp, 0.0);
}
}
if (!t->empty())
{
boost::mutex::scoped_lock lock(update_trajectory_message_);
trajectory_message_to_display_.swap(t);
if (interrupt_display_property_->getBool())
interruptCurrentDisplay();
}
}
在rivz插件中显示:
void TrajectoryVisualization::update(float wall_dt, float ros_dt)
{
if (drop_displaying_trajectory_)
{
animating_path_ = false;
displaying_trajectory_message_.reset();
trajectory_slider_panel_->update(0);
drop_displaying_trajectory_ = false;
}
if (!animating_path_)
{ // finished last animation?
boost::mutex::scoped_lock lock(update_trajectory_message_);
// new trajectory available to display?
if (trajectory_message_to_display_ && !trajectory_message_to_display_->empty())
{
animating_path_ = true;
displaying_trajectory_message_ = trajectory_message_to_display_;
changedShowTrail();
if (trajectory_slider_panel_)
trajectory_slider_panel_->update(trajectory_message_to_display_->getWayPointCount());
}
else if (displaying_trajectory_message_)
{
if (loop_display_property_->getBool())
{ // do loop? -> start over too
animating_path_ = true;
}
else if (trajectory_slider_panel_ && trajectory_