ROS编程（ETH）2018更新版习题说明（五）

内容要点：

ROS服务调用（Service calls）

练习与测试：

使用在习题2和习题3中实现的节点，并且新添加一个服务功能用于开启或停止机器人。此功能可以用作急停。

实现此功能参考（第4讲，第8页PPT），此任务中使用 std_srvs/SetBool 服务类型。
启动仿真并且调用服务在终端使用 rosservice call 启动或停止机器人。

提示与流程：

在HuskyHighlevelController.hpp中对应的位置添加如下代码：

#include <std_srvs/SetBool.h>

bool srvCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response);

ros::ServiceServer serviceServer_;

HuskyHighlevelController.cpp代码如下所示：

#include "husky_highlevel_controller/HuskyHighlevelController.hpp"

namespace husky_highlevel_controller {

    HuskyHighlevelController::HuskyHighlevelController(ros::NodeHandle& nodeHandle) : nodeHandle_(nodeHandle)
    {

	if (!readParameters()) 
        {
            ROS_ERROR("Could not read parameters.");
            ros::requestShutdown();
        }

        // subscribers
        scan_sub_ = nodeHandle_.subscribe(subscriberTopic_, queue_size , &HuskyHighlevelController::scanCallback, this);
        // publishers
        cmd_pub_  = nodeHandle_.advertise<geometry_msgs::Twist>("/cmd_vel",10);    
	vis_pub_  = nodeHandle_.advertise<visualization_msgs::Marker>("/visualization_marker",10);
	// service_server	
	serviceServer_ = nodeHandle_.advertiseService("husky_start_move", &HuskyHighlevelController::srvCallback, this);

	ROS_INFO("Successfully launched node.");

    }

    HuskyHighlevelController::~HuskyHighlevelController()
    {
    }

    bool HuskyHighlevelController::readParameters()
    {
        if (!nodeHandle_.getParam("scan_sub_topic", subscriberTopic_)) 
	{
		ROS_ERROR("Could not find scan_sub_topic parameter!");
		return false;
	}
       
        if (!nodeHandle_.getParam("scan_sub_queue_size", queue_size))
	{	
		ROS_ERROR("Could not find scan_sub_queue_size parameter!");
		return false;
	}
       
        return true;
    }

    void HuskyHighlevelController::scanCallback(const sensor_msgs::LaserScan &scan_msg)
    {
        float smallest_distance = 1000;
        // the angle corresponding to the minimum distance

        //number of the elements in ranges array
        int arr_size = floor((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment);
        for (int i=0 ; i< arr_size ;i++)
        {
            if (scan_msg.ranges[i] < smallest_distance)
            {
                smallest_distance = scan_msg.ranges[i];
		alpha_pillar = (scan_msg.angle_min + i*scan_msg.angle_increment);
            }
        }
        //Pillar Husky offset pose 
        x_pillar = smallest_distance*cos(alpha_pillar);
        y_pillar = smallest_distance*sin(alpha_pillar);

	//cout<<"cout Minimum laser distance(m): "<<smallest_distance<<"\n";
        //ROS_INFO_STREAM("ROS_INFO_STREAM Minimum laser distance(m): "<<smallest_distance);
	//ROS_INFO("Pillar laser distance(m):%lf", smallest_distance);
	ROS_INFO("Pillar offset angle(rad):%lf", alpha_pillar);
	ROS_INFO("pillar x distance(m):%lf", x_pillar);
	ROS_INFO("pillar y distance(m):%lf", y_pillar);

        //P-Controller to drive husky towards the pillar
        //propotinal gain
        float p_gain_vel = 0.1;
        float p_gain_ang = 0.4;

        if( start_move && x_pillar>0.2 )
        {
            if (x_pillar <= 0.4 )
            {
                 vel_msg_.linear.x = 0; 
                 vel_msg_.angular.z = 0;

            }
            else 
            {
//		 if(start_move)
//		 {
                 	vel_msg_.linear.x = x_pillar * p_gain_vel  ;
                 	vel_msg_.angular.z = -(y_pillar * p_gain_ang) ;
//		 }
//		 if(start_move==false)
//		 {
//                 	vel_msg_.linear.x = 0;
//                 	vel_msg_.angular.z = 0;		 
//		 }
             }
 
       }
       else
       {
                 vel_msg_.linear.x = 0;
                 vel_msg_.angular.z = 0;
       }
       cmd_pub_.publish(vel_msg_);

       //RViz Marker
       marker.header.frame_id = "base_laser"; //base no 
       marker.header.stamp = ros::Time();
       marker.ns = "pillar";
       marker.id = 0;
       marker.type = visualization_msgs::Marker::SPHERE;
       marker.action = visualization_msgs::Marker::ADD;
       marker.pose.position.x = x_pillar;
       marker.pose.position.y = y_pillar; 
       marker.scale.x = 0.2;
       marker.scale.y = 0.2;
       marker.scale.z = 2.0;
       marker.color.a = 1.0; // Don't forget to set the alpha!
       marker.color.r = 0.1;
       marker.color.g = 0.1;
       marker.color.b = 0.1;
       vis_pub_.publish(marker);

    }


    bool HuskyHighlevelController::srvCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response)
    {
//	try{
		if (request.data)
		{
		    start_move = true;
		}
		else
		{
		    start_move = false;
		}
		response.success = true;
//	}

//	catch(...){
//		ROS_WARN("Impossible to execute Start-Move service");
//		ros::Duration(1.0).sleep();
//		response.success = false;
//		//continue;
//        }

        ROS_INFO("request: %i", request.data );
        ROS_INFO("sending back response: [%i]", response.success); 
       
        return true;


    }

/*
    void HuskyHighlevelController::pController()
    {
        //propotinal gain
        float p_gain_vel = 0.4;
        float p_gain_ang = 1;
        if(start_move)
        {
            if (x_pillar <= 0.4 )
             {
                 vel_msg_.linear.x = 0; 
                 vel_msg_.angular.z =0;

             }
            else 
              {
                 vel_msg_.linear.x = x_pillar * p_gain_vel  ;
                 vel_msg_.angular.z = -alpha_pillar ;

              }
 
       }
       else
       {
                 vel_msg_.linear.x = 0;
                 vel_msg_.angular.z =0;
       }

        
    }
*/

} /* namespace */

代码仅供参考，实现效果如下图：