rviz marker测试程序

#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <chrono>
#include <thread>
#include <mutex>
#include <visualization_msgs/Marker.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "marker_tset_node");
    ros::NodeHandle nh;
    ROS_INFO("Begin to work.\n");
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1);

    while (1) {
        visualization_msgs::Marker marker;
        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = "/my_frame";
        marker.header.stamp = ros::Time::now();

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "basic_shapes";
        marker.id = 0;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        std::string type;
        nh.param<std::string>("type", type, "CUBE");
        marker.type = type == "CUBE" ? visualization_msgs::Marker::CUBE : visualization_msgs::Marker::CYLINDER;

        // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
        std::string action;
        nh.param<std::string>("action", action, "ADD");
        marker.action = action == "ADD" ? visualization_msgs::Marker::ADD :
                        action == "DELETE" ? visualization_msgs::Marker::DELETE :
                        visualization_msgs::Marker::DELETEALL;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        double position_x, position_y, position_z;
        nh.param<double>("position_x", position_x, 1.0);
        nh.param<double>("position_y", position_y, 1.0);
        nh.param<double>("position_z", position_z, 1.0);
        marker.pose.position.x = position_x;
        marker.pose.position.y = position_y;
        marker.pose.position.z = position_z;
        marker.pose.orientation.x = 0.0;
        marker.pose.orientation.y = 0.0;
        marker.pose.orientation.z = 0.0;
        marker.pose.orientation.w = 1.0;

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        double scale_x, scale_y, scale_z;
        nh.param<double>("scale_x", scale_x, 1.0);
        nh.param<double>("scale_y", scale_y, 1.0);
        nh.param<double>("scale_z", scale_z, 1.0);

        marker.scale.x = scale_x;
        marker.scale.y = scale_y;
        marker.scale.z = scale_z;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;
        marker.color.a = 1.0;

        marker.lifetime = ros::Duration();
        marker_pub.publish(marker);
    }
    return 0;
}