古月居 ROS入门21讲 第十四讲 服务器Server的编程实现

古月居 ROS入门21讲 第十四讲 服务器Server的编程实现

• C++
  turtle_command_server.cpp

#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<std_srvs/Trigger.h>

ros::Publisher turtle_vel_pub;
bool pubCommand=false;

bool commandCallback(std_srvs::Trigger::Request &req,
		std_srvs::Trigger::Response &res)
{
    pubCommand=!pubCommand;
    ROS_INFO("Publish turtle velocity command [%s]",pubCommand==false?"No":"Yes");
    res.success=true;
    res.message="Change turtle command state!";
    return true;
}
int main(int argc,char**argv)
{
    ros::init(argc,argv,"turtle_command_server");
    ros::NodeHandle n;
    ros::ServiceServer command_service=n.advertiseService("/turtle_command",commandCallback);
    turtle_vel_pub=n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ROS_INFO("Ready to receive turtle command.");
    ros::Rate rate(10);
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x=0.5;
    vel_msg.angular.z=0.2;
    while(ros::ok())
    {
        ros::spinOnce();
	if(pubCommand)
	{
	    turtle_vel_pub.publish(vel_msg);

	}
	rate.sleep();
    }
    return 0;
}

• Python
  turtle_command_server.py

#!/usr/bin/python
# -*- coding:utf-8 -*-

import rospy
import thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger,TriggerResponse

pubCommand=False
turtle_vel_pub=rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=10)

def command_thread():
    vel_msg=Twist()
    vel_msg.linear.x=0.4
    vel_msg.angular.z=0.2
   # rate=rospy.Rate(10)
    while True:
        if pubCommand:
            turtle_vel_pub.publish(vel_msg)
       # rate.sleep()
        time.sleep(0.1)

def commandCallback(req):
    #声明是全局变量，因为下面对其赋值，为了区分和局部变量区分
    global pubCommand
    pubCommand=bool(1-pubCommand)

    rospy.loginfo("Publish turtle velocity command[%d]",pubCommand)

    return TriggerResponse(1,"Change turtle command state!")

def turtle_command_server():
    rospy.init_node("turtle_command_server")
    s=rospy.Service("/turtle_command",Trigger,commandCallback)
    print "Ready to receive turtle command."
    thread.start_new_thread(command_thread,())
    rospy.spin()
if __name__=="__main__":
    turtle_command_server()