本文是將《ROS在rviz中實時顯示軌跡和點》博客中rviz軌跡顯示轉為ROS2環境中的rviz2顯示。

ros2的工作空間創建這里就不展示了。

包的創建

ros2 pkg create --build-type ament_cmake showpath --dependencies rclcpp nav_msgs geometry_msgs tf2_geometry_msgs

showpath.cpp文檔創建

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <cstdlib>
#include <cmath>int main(int argc, char **argv)
{rclcpp::init(argc, argv);auto node = std::make_shared<rclcpp::Node>("showpath");auto path_pub = node->create_publisher<nav_msgs::msg::Path>("trajectory", 1);auto point_pub = node->create_publisher<geometry_msgs::msg::PointStamped>("point", 1);rclcpp::Time current_time, last_time;current_time = node->now();last_time = node->now();nav_msgs::msg::Path path;path.header.stamp = current_time;path.header.frame_id = "odom";double x = 0.0;double y = 0.0;double z = 0.0;double th = 0.0;double vx = 0.1 + 0.2 * std::rand() / double(RAND_MAX);double vy = -0.1 + 0.2 * std::rand() / double(RAND_MAX);double vth = 0.1;rclcpp::Rate loop_rate(10);while (rclcpp::ok()){current_time = node->now();double dt = 1.0;double delta_x = (vx * std::cos(th) - vy * std::sin(th)) * dt;double delta_y = (vx * std::sin(th) + vy * std::cos(th)) * dt;double delta_th = vth * dt;x += delta_x;y += delta_y;z += 0.005;th += delta_th;geometry_msgs::msg::PoseStamped this_pose_stamped;geometry_msgs::msg::PointStamped this_point_stamped;this_pose_stamped.pose.position.x = x;this_pose_stamped.pose.position.y = y;this_pose_stamped.pose.position.z = z;this_point_stamped.header.stamp = current_time;this_point_stamped.header.frame_id = "odom";this_point_stamped.point.x = x;this_point_stamped.point.y = y;this_point_stamped.point.z = z;RCLCPP_INFO(node->get_logger(), "current_x: %f", x);RCLCPP_INFO(node->get_logger(), "current_y: %f", y);RCLCPP_INFO(node->get_logger(), "pos: (%f, %f)", x, y);// auto goal_quat = tf2::createQuaternionMsgFromYaw(th);// this_pose_stamped.pose.orientation = goal_quat;tf2::Quaternion orientation;orientation.setRPY(0, 0, th);  geometry_msgs::msg::Quaternion orientation_msg = tf2::toMsg(orientation);this_pose_stamped.pose.orientation = orientation_msg;this_pose_stamped.header.stamp = current_time;this_pose_stamped.header.frame_id = "odom";path.poses.push_back(this_pose_stamped);path_pub->publish(path);point_pub->publish(this_point_stamped);rclcpp::spin_some(node);last_time = current_time;loop_rate.sleep();}rclcpp::shutdown();return 0;
}

CMakeLists.txt中添加

add_executable(showpath src/showpath.cpp)
ament_target_dependencies(showpath rclcpp nav_msgs geometry_msgs tf2_geometry_msgs)install(TARGETSshowpathDESTINATION lib/${PROJECT_NAME})

編譯

colcon build

運行

source install/setup.bash
ros2 run showpath showpath

運行rviz2

ros2 run rviz2 rviz2

rviz配置見https://blog.csdn.net/gophae/article/details/108514336

效果