Loading...

ROS入门(二)——服务和全局参数

ROS入门(二)——服务和全局参数
  iwehdio的博客园:https://www.cnblogs.com/iwehdio/

1、服务的实现

  • 客户端 Client 的实现:

    • 创建功能包(工作空间src目录下):catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

    • 初始化 ROS 节点,赋予节点名称。

    • 查找系统中是否有所给出的服务名,并等待直到服务存在。

    • 创建客户端,用于向服务发送请求。

    • 初始化请求数据。

    • 请求服务调用,并等待直到收到响应数据。

    • C++ 实现:

      • 设置编译规则:

        add_executable(turtle_spawn src/turtle_spawn.cpp)
        target_link_libraries(turtle_spawn ${catkin_LIBRARIES})
        
      • C++ 源代码:

        /**
         * 该例程将请求/spawn服务,服务数据类型turtlesim::Spawn
         */
        
        #include <ros/ros.h>
        #include <turtlesim/Spawn.h>
        
        int main(int argc, char** argv)
        {
            // 初始化ROS节点
        	ros::init(argc, argv, "turtle_spawn");
        
            // 创建节点句柄
        	ros::NodeHandle node;
        
            // 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
        	ros::service::waitForService("/spawn");
        	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
        
            // 初始化turtlesim::Spawn的请求数据
        	turtlesim::Spawn srv;
        	srv.request.x = 2.0;
        	srv.request.y = 2.0;
        	srv.request.name = "turtle2";
        
            // 请求服务调用
        	ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
        			 srv.request.x, srv.request.y, srv.request.name.c_str());
        
        	add_turtle.call(srv);
        
        	// 显示服务调用结果
        	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());
        
        	return 0;
        };
        
      • 编译并运行。

    • Python 实现:

      #!/usr/bin/env python
      # -*- coding: utf-8 -*-
      # 该例程将请求/spawn服务,服务数据类型turtlesim::Spawn
      
      import sys
      import rospy
      from turtlesim.srv import Spawn
      
      def turtle_spawn():
      	# ROS节点初始化
          rospy.init_node('turtle_spawn')
      
      	# 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
          rospy.wait_for_service('/spawn')
          try:
              add_turtle = rospy.ServiceProxy('/spawn', Spawn)
      
      		# 请求服务调用,输入请求数据
              response = add_turtle(2.0, 2.0, 0.0, "turtle2")
              return response.name
          except rospy.ServiceException, e:
              print "Service call failed: %s"%e
      
      if __name__ == "__main__":
      	#服务调用并显示调用结果
          print "Spwan turtle successfully [name:%s]" %(turtle_spawn())
      
  • 服务器 Server 的实现:

    • 初始化 ROS 节点,赋予节点名称。

    • 创建服务端 Server 实例。

    • 循环等待服务请求,获取到请求后进入回调函数。

    • 完成功能处理,在回调函数中应答数据。

    • C++ 实现:

      • 设置编译规则:

        add_executable(turtle_command_server src/turtle_command_server.cpp)
        target_link_libraries(turtle_command_server ${catkin_LIBRARIES})
        
      • C++ 源代码:

        /**
         * 该例程将执行/turtle_command服务,服务数据类型std_srvs/Trigger
         */
         
        #include <ros/ros.h>
        #include <geometry_msgs/Twist.h>
        #include <std_srvs/Trigger.h>
        
        ros::Publisher turtle_vel_pub;
        bool pubCommand = false;
        
        // service回调函数,输入参数req,输出参数res
        bool commandCallback(std_srvs::Trigger::Request  &req,
                 			std_srvs::Trigger::Response &res)
        {
        	pubCommand = !pubCommand;
        
            // 显示请求数据
            ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");
        
        	// 设置反馈数据
        	res.success = true;
        	res.message = "Change turtle command state!"
        
            return true;
        }
        
        int main(int argc, char **argv)
        {
            // ROS节点初始化
            ros::init(argc, argv, "turtle_command_server");
        
            // 创建节点句柄
            ros::NodeHandle n;
        
            // 创建一个名为/turtle_command的server,注册回调函数commandCallback
            ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);
        
        	// 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
        	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
        
            // 循环等待回调函数
            ROS_INFO("Ready to receive turtle command.");
        
        	// 设置循环的频率
        	ros::Rate loop_rate(10);
        
        	while(ros::ok())
        	{
        		// 查看一次回调函数队列
            	ros::spinOnce();
        		
        		// 如果标志为true,则发布速度指令
        		if(pubCommand)
        		{
        			geometry_msgs::Twist vel_msg;
        			vel_msg.linear.x = 0.5;
        			vel_msg.angular.z = 0.2;
        			turtle_vel_pub.publish(vel_msg);
        		}
        
        		//按照循环频率延时
        	    loop_rate.sleep();
        	}
        
            return 0;
        }
        
      • 运行:

        • 运行小海龟节点。

        • 运行服务器端,并用终端给服务器发送请求。

          $ rosrun learning_service turtle_command_server
          $ rosservice call /turtle_command "{}"
          
        • std_srvs/Trigger 的数据结构为,请求为空,响应为一个是否响应的布尔值和一个字符串信息。

    • Python 实现:

      • Python 源代码:

        #!/usr/bin/env python
        # -*- coding: utf-8 -*-
        # 该例程将执行/turtle_command服务,服务数据类型std_srvs/Trigger
        
        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():	
        	while True:
        		if pubCommand:
        			vel_msg = Twist()
        			vel_msg.linear.x = 0.5
        			vel_msg.angular.z = 0.2
        			turtle_vel_pub.publish(vel_msg)
        			
        		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():
        	# ROS节点初始化
            rospy.init_node('turtle_command_server')
        
        	# 创建一个名为/turtle_command的server,注册回调函数commandCallback
            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()
        
        • 执行多线程时会出错,虽然不影响程序运行。
  • 服务数据的定义与使用(自定义话题消息)::

    • 需要将数据用三条横线---区分,上方为 request 请求,下方为 response 响应数据。

    • 创建 srv 文件夹并在其下创建 Person.srv 文件(touch Person.srv),并消息格式为:

      string name
      uint8 age
      uint8 sex
      uint8 unknown = 0
      uint8 male = 1
      uint8 female = 2
      ---
      string result
      
    • 在 package.xml 中添加功能包依赖。即编译依赖 message_generation,运行依赖 message_runtime。

      <build_depend>message_generation</build_depend>
      <exec_depend>message_runtime</exec_depend>
      
    • 在 CMakeLists.txt 中添加编译选项。

      • 在 find_package() 中添加 message_generation

      • 定义消息接口,在 Declare ROS messages, services and actions 下。这里与 msg 不同的是,使用的是add_service_files()而不是add_message_files()

        add_service_files(FILES Person.srv)
        generate_messages(DEPENDENCIES std_msgs)
        
        • 第一行表示将 Person.msg 作为一个消息接口,第二行表示该消息接口的依赖。
      • 在 catkin_package 中添加运行时依赖。

        catkin_package(
           CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
        )
        
        • 去掉 CATKIN_DEPENDS 前的注释,并在其后添加 message _runtime。
    • 创建发布者和订阅者(C++实现):

      • 在 CMakeLists.txt 中配置编译信息。

        add_executable(person_server src/person_server.cpp)
        target_link_libraries(person_server ${catkin_LIBRARIES})
        add_dependencies(person_server ${PROJECT_NAME}_gencpp)
        
        add_executable(person_client src/person_client.cpp)
        target_link_libraries(person_client ${catkin_LIBRARIES})
        add_dependencies(person_client ${PROJECT_NAME}_gencpp)
        
      • 客户端:

        /**
         * 该例程将请求/show_person服务,服务数据类型learning_service::Person
         */
        
        #include <ros/ros.h>
        #include "learning_service/Person.h"
        
        int main(int argc, char** argv)
        {
            // 初始化ROS节点
        	ros::init(argc, argv, "person_client");
        
            // 创建节点句柄
        	ros::NodeHandle node;
        
            // 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
        	ros::service::waitForService("/show_person");
        	ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");
        
            // 初始化learning_service::Person的请求数据
        	learning_service::Person srv;
        	srv.request.name = "Tom";
        	srv.request.age  = 20;
        	srv.request.sex  = learning_service::Person::Request::male;
        
            // 请求服务调用
        	ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
        			 srv.request.name.c_str(), srv.request.age, srv.request.sex);
        
        	person_client.call(srv);
        
        	// 显示服务调用结果
        	ROS_INFO("Show person result : %s", srv.response.result.c_str());
        
        	return 0;
        };
        
      • 服务端:

        /**
         * 该例程将执行/show_person服务,服务数据类型learning_service::Person
         */
         
        #include <ros/ros.h>
        #include "learning_service/Person.h"
        
        // service回调函数,输入参数req,输出参数res
        bool personCallback(learning_service::Person::Request  &req,
                 			learning_service::Person::Response &res)
        {
            // 显示请求数据
            ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);
        
        	// 设置反馈数据
        	res.result = "OK";
        
            return true;
        }
        
        int main(int argc, char **argv)
        {
            // ROS节点初始化
            ros::init(argc, argv, "person_server");
        
            // 创建节点句柄
            ros::NodeHandle n;
        
            // 创建一个名为/show_person的server,注册回调函数personCallback
            ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
        
            // 循环等待回调函数
            ROS_INFO("Ready to show person informtion.");
            ros::spin();
        
            return 0;
        }
        
      • 编译代码后运行。

        $ roscore
        $ rosrun learning_service person_server
        $ rosrun learning_service person_client 
        
    • Python 实现:

      • 客户端:

        #!/usr/bin/env python
        # -*- coding: utf-8 -*-
        # 该例程将请求/show_person服务,服务数据类型learning_service::Person
        
        import sys
        import rospy
        from learning_service.srv import Person, PersonRequest
        
        def person_client():
        	# ROS节点初始化
            rospy.init_node('person_client')
        
        	# 发现/show_person服务后,创建一个服务客户端,连接名为/show_person的service
            rospy.wait_for_service('/show_person')
            try:
                person_client = rospy.ServiceProxy('/show_person', Person)
        
        		# 请求服务调用,输入请求数据
                response = person_client("Tom", 20, PersonRequest.male)
                return response.result
            except rospy.ServiceException, e:
                print "Service call failed: %s"%e
        
        if __name__ == "__main__":
        	#服务调用并显示调用结果
            print "Show person result : %s" %(person_client())
        
      • 服务端:

        #!/usr/bin/env python
        # -*- coding: utf-8 -*-
        # 该例程将执行/show_person服务,服务数据类型learning_service::Person
        
        import rospy
        from learning_service.srv import Person, PersonResponse
        
        def personCallback(req):
        	# 显示请求数据
            rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)
        
        	# 反馈数据
            return PersonResponse("OK")
        
        def person_server():
        	# ROS节点初始化
            rospy.init_node('person_server')
        
        	# 创建一个名为/show_person的server,注册回调函数personCallback
            s = rospy.Service('/show_person', Person, personCallback)
        
        	# 循环等待回调函数
            print "Ready to show person informtion."
            rospy.spin()
        
        if __name__ == "__main__":
            person_server()
        
      • Python 运行总是报错:

        Exception in thread Thread-3:
        Traceback (most recent call last):
          File "/usr/lib/python2.7/threading.py", line 801, in __bootstrap_inner
            self.run()
          File "/usr/lib/python2.7/threading.py", line 754, in run
            self.__target(*self.__args, **self.__kwargs)
          File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 157, in run
            except ConnectionAbortedError:
        NameError: global name 'ConnectionAbortedError' is not defined
        

2、全局参数

  • rosparam 命令:

    • 列出当前所有参数:$ rosparam list
    • 显示某个参数值:$ rosparam get param_key
    • 设置某个参数值:$ rosparam set param_key param_value
    • 保存参数到文件:$ rosparam dump file_name
    • 从文件读取参数:$ rosparam load file_name
    • 删除参数:$ rosparam delete param_key
  • C++ 操作全局参数:

    /**
     * 该例程设置/读取海龟例程中的参数
     */
    #include <string>
    #include <ros/ros.h>
    #include <std_srvs/Empty.h>
    
    int main(int argc, char **argv)
    {
    	int red, green, blue;
    
        // ROS节点初始化
        ros::init(argc, argv, "parameter_config");
    
        // 创建节点句柄
        ros::NodeHandle node;
    
        // 读取背景颜色参数
    	ros::param::get("/turtlesim/background_r", red);
    	ros::param::get("/turtlesim/background_g", green);
    	ros::param::get("/turtlesim/background_b", blue);
    
    	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);
    
    	// 设置背景颜色参数
    	ros::param::set("/turtlesim/background_r", 255);
    	ros::param::set("/turtlesim/background_g", 255);
    	ros::param::set("/turtlesim/background_b", 255);
    
    	ROS_INFO("Set Backgroud Color[255, 255, 255]");
    
        // 读取背景颜色参数
    	ros::param::get("/turtlesim/background_r", red);
    	ros::param::get("/turtlesim/background_g", green);
    	ros::param::get("/turtlesim/background_b", blue);
    
    	ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);
    
    	// 调用服务,刷新背景颜色
    	ros::service::waitForService("/clear");
    	ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
    	std_srvs::Empty srv;
    	clear_background.call(srv);
    	
    	sleep(1);
    
        return 0;
    }
    
    • 设置编译规则:

      add_executable(parameter_config src/parameter_config.cpp)
      target_link_libraries(parameter_config ${catkin_LIBRARIES})
      
  • Python 操作全局参数:

    #!/usr/bin/env python
    # -*- coding: utf-8 -*-
    # 该例程设置/读取海龟例程中的参数
    
    import sys
    import rospy
    from std_srvs.srv import Empty
    
    def parameter_config():
    	# ROS节点初始化
        rospy.init_node('parameter_config', anonymous=True)
    
    	# 读取背景颜色参数
        red   = rospy.get_param('/turtlesim/background_r')
        green = rospy.get_param('/turtlesim/background_g')
        blue  = rospy.get_param('/turtlesim/background_b')
    
        rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)
    
    	# 设置背景颜色参数
        rospy.set_param("/turtlesim/background_r", 255);
        rospy.set_param("/turtlesim/background_g", 255);
        rospy.set_param("/turtlesim/background_b", 255);
    
        rospy.loginfo("Set Backgroud Color[255, 255, 255]");
    
    	# 读取背景颜色参数
        red   = rospy.get_param('/turtlesim/background_r')
        green = rospy.get_param('/turtlesim/background_g')
        blue  = rospy.get_param('/turtlesim/background_b')
    
        rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)
    
    	# 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
        rospy.wait_for_service('/clear')
        try:
            clear_background = rospy.ServiceProxy('/clear', Empty)
    
    		# 请求服务调用,输入请求数据
            response = clear_background()
            return response
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
    
    if __name__ == "__main__":
        parameter_config()
    
    • 如果出错,先用 rosparam 查看参数列表,看看参数名是否写错。

参考:古月ROS入门21讲:https://www.bilibili.com/video/BV1zt411G7Vn?p=21

iwehdio的博客园:https://www.cnblogs.com/iwehdio/

posted @ 2020-04-17 17:39  iwehdio  阅读(1504)  评论(0编辑  收藏  举报