ROS中的訊息和服務

NO IMAGE

1、路徑視覺化釋出

path_pub_ = nh.advertise<nav_msgs::Path>("path_view",10);    
    nav_msgs::Path path;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "/map";
    float j = 0;
    for(int i = 0; i < 300;   i){
        j  = 0.1;
        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = j;
        pose.pose.position.y = j;
        pose.pose.position.z = 0;
        path.poses.push_back(pose);
    }
    path_pub_.publish(path);

2、marker釋出

    
  markers_pub_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 20);
visualization_msgs::Marker m;
    m.header.stamp = ros::Time::now();
    m.header.frame_id = "/map";
    m.id = 0;
    m.type = visualization_msgs::Marker::LINE_STRIP;;
    m.scale.x = .1;
    m.scale.y = .1;
    m.scale.z = .1;
    m.color.a = 1;
    m.lifetime = ros::Duration(0.5);
    m.color.r = 1;
    m.color.g = 1;
    float k = -1;
    for(int i = 0; i < 100;   i)
    {
        k -= 0.1;
        geometry_msgs::Point p;
        p.x = k;
        p.y = k;
        p.z = 0;
        m.points.push_back(p);
    }
    markers_pub_.publish(m);

3、請求make_plan服務

#include <nav_msgs/GetPlan.h>
ros::ServiceClient serviceClient;
serviceClient = nh.serviceClient<nav_msgs::GetPlan>("move_base/make_plan", true);

int class::request_plan(void)
{
    nav_msgs::GetPlan srv;
    fillPathRequest(srv.request);
    if(serviceClient.call(srv))
    {
        callPlanningService(serviceClient, srv);
    }
    else
    {
        ROS_ERROR("Failed to call service!");
    }
    return 0;
}
void class::callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
    if (!srv.response.plan.poses.empty()) {
        BOOST_FOREACH(const geometry_msgs::PoseStamped &p, srv.response.plan.poses) {
            ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
        }
    }
    else {
        ROS_WARN("Got empty plan");
    }
}
void class::fillPathRequest(nav_msgs::GetPlan::Request &request)
{
    request.start.header.frame_id ="map";
    request.start.pose.position.x = -1;//初始位置x座標
    request.start.pose.position.y = -1;//初始位置y座標
    request.start.pose.orientation.w = 1.0;//方向
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = -10;//終點座標
    request.goal.pose.position.y = -10;
    request.goal.pose.orientation.w = 1.0;
    request.tolerance = 0.5;//如果不能到達目標,最近可到的約束
}