本文是将《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(TARGETS
  showpath
  DESTINATION 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

效果