合并服务端代码

src/ros_test_pkg/CMakeLists.txt

@@ -8,9 +8,12 @@ project(ros_test_pkg)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
   roscpp
   std_msgs
   message_generation
+  actionlib
+  actionlib_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -53,23 +56,22 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  mySrv.srv
+)
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(
+  FILES
+  myAction.action
+)
 
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   std_msgs
+  actionlib_msgs
 )
 
 ################################################
@@ -87,10 +89,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/dynamic.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -136,6 +137,13 @@ include_directories(
 add_executable(publish_node src/topic_publisher.cpp)
 add_executable(subscribe_node src/topic_subscriber.cpp)
 
+add_executable(server_node src/server_node.cpp)
+add_executable(client_node src/client_node.cpp)
+
+add_executable(turtlesim_control_node src/control_turtle.cpp)
+
+add_executable(action_server src/action_server.cpp)
+add_executable(action_client src/action_client.cpp)
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -144,9 +152,17 @@ add_executable(subscribe_node src/topic_subscriber.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(publish_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(subscribe_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+add_dependencies(server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(client_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+add_dependencies(action_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(action_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+add_dependencies(turtlesim_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 ## Specify libraries to link a library or executable target against
 target_link_libraries(publish_node
   ${catkin_LIBRARIES}
@@ -155,6 +171,23 @@ target_link_libraries(subscribe_node
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(server_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(client_node
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(turtlesim_control_node
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(action_server
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(action_client
+  ${catkin_LIBRARIES}
+)
 #############
 ## Install ##
 #############

src/ros_test_pkg/action/myAction.action

@@ -0,0 +1,5 @@
+int32 goal_num
+---
+int32 result_num
+---
+int32 current_num

src/ros_test_pkg/cfg/config.yaml

@@ -0,0 +1,6 @@
+# this is comment
+cmd_topic: /cmd_vel
+linear_x: 0.15
+linear_y: -0.25
+angular_z: 1.0
+loop_rate: 5

src/ros_test_pkg/cfg/dynamic.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+PACKAGE = "ros_test_pkg"
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("cmd_topic",    str_t,    0, "turtlesim cmd topic", "/turtle1/cmd_vel")
+gen.add("cmd_pub_rate", int_t,    0, "turtlesim cmd pub rate", 1, 0, 5)
+gen.add("linear_x",     double_t, 0, "turtlesim linear.x", 1.0, 0.5, 3.0)
+gen.add("angular_z",    double_t, 0, "turtlesim angular.z", 1.0, 0.5, 3.0)
+gen.add("move_flag",    bool_t,   0, "turtlesim whether move or not", True)
+
+log_enum = gen.enum([ gen.const("info", int_t, 0, "log print flag:INFO"),
+                      gen.const("warn", int_t, 1, "log print flag:WARN"),
+                      gen.const("error", int_t, 2, "log print flag:ERROR")],
+                      "Set Log Level")
+
+gen.add("log_level", int_t, 0, "Set Log Level", 0, 0, 2, edit_method=log_enum)
+
+exit(gen.generate(PACKAGE, "turtlesim_control_node", "dynamic"))

src/ros_test_pkg/launch/new_test.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" output="screen" />
+</launch>

src/ros_test_pkg/launch/srv_test.launch

@@ -0,0 +1,12 @@
+<!--
+  Copyright:www.corvin.cn
+  Description:startup service test.
+  Author:corvin
+  History:
+-->
+<launch>
+   <node pkg="ros_test_pkg" type="server_node" name="server_node" output="screen" />
+   <node pkg="ros_test_pkg" type="client_node" name="client_node" output="screen" />
+</launch>
+
+

src/ros_test_pkg/launch/start_test.launch

@@ -0,0 +1,17 @@
+<!--
+ Description:start test launch.
+ Author:corvin
+ History:20210515:init this launch file.
+-->
+
+<launch>
+  <node pkg="ros_test_pkg" type="publish_node" name="test_publish_node" output="screen" respawn="true" >
+    <param name="name" value="HelloWorld" />
+    <param name="id" value="999" />
+    <param name="percent" value="0.666" />
+  </node>
+
+  <node pkg="ros_test_pkg" type="subscribe_node" name="test_subscribe_node" output="screen" required="true" />
+
+  <include file="$(find ros_test_pkg)/launch/new_test.launch" />
+</launch>

src/ros_test_pkg/launch/start_turtle.launch

@@ -0,0 +1,10 @@
+<launch>
+    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" output="screen" />
+
+    <node pkg="ros_test_pkg" type="turtlesim_control_node" name="contorl_node" output="screen">
+      <rosparam file="$(find ros_test_pkg)/cfg/config.yaml" command="load" />
+    </node>
+
+    <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="rqt_reconfigure" output="screen" />
+</launch>
+

src/ros_test_pkg/package.xml

@@ -52,11 +52,15 @@
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>message_runtime</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/ros_test_pkg/src/action_client.cpp

@@ -0,0 +1,54 @@
+#include <ros/ros.h>
+#include <actionlib/client/simple_action_client.h>
+#include "ros_test_pkg/myActionAction.h"
+
+
+void finishCB(const actionlib::SimpleClientGoalState &state,
+        const ros_test_pkg::myActionResultConstPtr &result)
+{
+    ROS_WARN("Action status: [%s], result: [%d]", state.toString().c_str(), result->result_num);
+    ros::shutdown();
+}
+
+void startCB()
+{
+    ROS_INFO("ActionServer start action...");
+}
+
+void feedbackCB(const ros_test_pkg::myActionFeedbackConstPtr &feedback)
+{
+    ROS_INFO("Client get feedback: [%d]", feedback->current_num);
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "action_client");
+    if(argc != 3)
+    {
+        ROS_INFO("%d", argc);
+        ROS_WARN("Usage: action_client <goal> <time>");
+        return 1;
+    }
+
+    actionlib::SimpleActionClient<ros_test_pkg::myActionAction> ac("action_server", true);
+    ROS_INFO("Waiting for action server to start...");
+    ac.waitForServer();
+
+    ros_test_pkg::myActionGoal goal;
+    goal.goal_num = atoi(argv[1]);
+    ROS_INFO("Sending Goal [%d] and Preempt time of [%d]s", goal.goal_num, atoi(argv[2]));
+    ac.sendGoal(goal, &finishCB, &startCB, &feedbackCB);
+
+    bool finished_before_timeout =ac.waitForResult(ros::Duration(atoi(argv[2])));
+    actionlib::SimpleClientGoalState state = ac.getState();
+    ROS_INFO("Action status: [%s]", state.toString().c_str());
+    if(!finished_before_timeout)
+    {
+        ac.cancelGoal();
+        ROS_INFO("Action status:[%s]", state.toString().c_str());
+        ROS_ERROR("Action did't finish before the timeout.");
+    }
+
+    return 0;
+}
+

src/ros_test_pkg/src/action_server.cpp

@@ -0,0 +1,70 @@
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include "ros_test_pkg/myActionAction.h"
+
+class myActionAction
+{
+    protected:
+        ros::NodeHandle nh_;
+        actionlib::SimpleActionServer<ros_test_pkg::myActionAction> as;
+
+        ros_test_pkg::myActionFeedback feedback;
+        ros_test_pkg::myActionResult result;
+
+        std::string action_name;
+        int goal;
+        int progress;
+
+    public:
+        myActionAction(std::string name):
+            as(nh_, name, boost::bind(&myActionAction::executeCB, this, _1), false),
+            action_name(name)
+    {
+        as.registerPreemptCallback(boost::bind(&myActionAction::preemptCB, this));
+        as.start();
+    }
+
+    void preemptCB()
+    {
+        ROS_ERROR("[%s] get preempted !", action_name.c_str());
+        result.result_num = progress;
+        as.setPreempted(result, "I got Preempted");
+    }
+
+    void executeCB(const ros_test_pkg::myActionGoalConstPtr &goal)
+    {
+        ROS_INFO("%s is processing the goal %d", action_name.c_str(), goal->goal_num);
+
+        ros::Rate rate(5);
+        for(progress = 1; progress <= goal->goal_num; progress++)
+        {
+            if(!as.isActive() || as.isPreemptRequested()) return;
+
+            if(goal->goal_num == progress)
+            {
+                result.result_num = progress;
+                as.setSucceeded(result);
+                ROS_INFO("[%s] Succeeded Goal [%d]", action_name.c_str(), progress);
+            }
+            else
+            {
+                feedback.current_num = progress;
+                as.publishFeedback(feedback);
+                ROS_INFO("Server send feedback: [%d]", feedback.current_num);
+            }
+            rate.sleep();
+        }
+    }
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "action_server");
+
+    myActionAction action_obj(ros::this_node::getName());
+    ROS_INFO("Starting Action Server ...");
+
+    ros::spin();
+    return 0;
+}
+

src/ros_test_pkg/src/client_node.cpp

@@ -0,0 +1,32 @@
+#include <ros/ros.h>
+#include "ros_test_pkg/mySrv.h"
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "client_node");
+    ros::NodeHandle handle;
+    ros_test_pkg::mySrv srv;
+
+    ros::Rate loop_rate(0.5);
+    ros::ServiceClient client = handle.serviceClient<ros_test_pkg::mySrv>("sum_service");
+    while(ros::ok())
+    {
+        srv.request.num_a = rand()%100;
+        srv.request.num_b = rand()%100;
+        ROS_INFO("Client Request:%d %d",srv.request.num_a, srv.request.num_b);
+
+        if(client.call(srv))
+        {
+            ROS_INFO("Server response:%s", srv.response.str.c_str());
+            ROS_INFO("---------------------------------");
+        }
+        else {
+            ROS_ERROR("request error !");
+            return 1;
+        }
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}

src/ros_test_pkg/src/control_turtle.cpp

@@ -0,0 +1,85 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros_test_pkg/dynamicConfig.h>
+
+using namespace std;
+
+string cmd_topic = "/turtle1/cmd_vel";
+int loop_rate = 1;
+double linear_x = 1.0;
+double linear_y = 1.0;
+double angular_z = 1.0;
+bool move_flag = true;
+int log_level = 0;
+
+void dynamic_callback(ros_test_pkg::dynamicConfig &config)
+{
+    ROS_INFO("Reconfigure Request:%s %d %f %f %s %d",
+            config.cmd_topic.c_str(),
+            config.cmd_pub_rate,
+            config.linear_x,
+            config.angular_z,
+            config.move_flag?"True":"False",
+            config.log_level);
+
+    cmd_topic = config.cmd_topic;
+    loop_rate = config.cmd_pub_rate;
+    linear_x  = config.linear_x;
+    angular_z = config.angular_z;
+    move_flag = config.move_flag;
+    log_level = config.log_level;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "turtlesim_control_move");
+    ros::NodeHandle handle;
+
+    ros::param::get("~cmd_topic", cmd_topic);
+    ros::param::get("~linear_x", linear_x);
+    ros::param::get("~linear_y", linear_y);
+    ros::param::get("~angular_z", angular_z);
+    ros::param::get("~loop_rate", loop_rate);
+
+    dynamic_reconfigure::Server<ros_test_pkg::dynamicConfig> server;
+    dynamic_reconfigure::Server<ros_test_pkg::dynamicConfig>::CallbackType callback;
+
+    callback = boost::bind(&dynamic_callback, _1);
+    server.setCallback(callback);
+
+    ros::Publisher cmdVelPub;
+    geometry_msgs::Twist speed;
+
+    while(ros::ok())
+    {
+        cmdVelPub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+        ros::Rate rate(loop_rate);
+        if(move_flag)
+        {
+            speed.linear.x  = linear_x;
+            speed.linear.y  = linear_y;
+            speed.angular.z = angular_z;
+            cmdVelPub.publish(speed);
+
+            switch(log_level)
+            {
+                case 0:
+                    ROS_INFO("Log level:INFO, cmd_pub_rate:%d", loop_rate);
+                    break;
+                case 1:
+                    ROS_WARN("Log level:WARN, cmd_pub_rate:%d", loop_rate);
+                    break;
+                case 2:
+                    ROS_ERROR("Log level:ERROR, cmd_pub_rate:%d", loop_rate);
+                    break;
+                default:
+                    break;
+            }
+        }
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    return 0;
+}

src/ros_test_pkg/src/server_node.cpp

@@ -0,0 +1,30 @@
+#include <ros/ros.h>
+#include <sstream>
+#include "ros_test_pkg/mySrv.h"
+
+bool service_callback(ros_test_pkg::mySrv::Request &req,
+                      ros_test_pkg::mySrv::Response &res)
+{
+    std::stringstream ss;
+    ROS_INFO("From client get:%d, %d", req.num_a, req.num_b);
+    int sum = req.num_a + req.num_b;
+    ss << "Sum is :" <<sum;
+    res.str = ss.str();
+
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "server_node");
+    ros::NodeHandle handle;
+
+    ros::ServiceServer service = handle.advertiseService("sum_service", service_callback);
+    ROS_INFO("Ready to recevie request from client...");
+
+    ros::spin();
+    return 0;
+}
+
+
+

src/ros_test_pkg/src/topic_publisher.cpp

@@ -1,29 +1,45 @@
 #include "ros/ros.h"
 #include "stdio.h"
 #include "ros_test_pkg/myMsg.h"
+#include <std_msgs/Int32.h>
 
 int main(int argc, char **argv)
 {
+    std::string param_name;
+    int param_id;
+    float param_percent;
+
     ros::init(argc, argv, "topic_publisher");
     ros::NodeHandle node_handle;
 
     ros::Publisher pub_number = node_handle.advertise<ros_test_pkg::myMsg>("/count", 10);
+    ros::Publisher pub_test = node_handle.advertise<std_msgs::Int32>("/int_test", 10);
+
+    ros::param::get("~name", param_name);
+    ros::param::get("~id", param_id);
+    ros::param::get("~percent", param_percent);
+
+    ROS_WARN("Name:%s,ID:%d,Percent:%f",param_name.c_str(),param_id,param_percent);
 
     ros::Rate rate(1);
-    int number_count = 0;
+    int count = 0;
     while (ros::ok())
     {
         ros_test_pkg::myMsg msg;
-        msg.name = "robot";
-        msg.device_id = number_count%5;
-        msg.percent = rand()/double(RAND_MAX);
+        msg.name = param_name;
+        msg.device_id = param_id;
+        msg.percent = param_percent;
         pub_number.publish(msg);
 
+        std_msgs::Int32 test_msg;
+        test_msg.data = count;
+        pub_test.publish(test_msg);
+
         ros::spinOnce();
         rate.sleep();
-        number_count++;
+        count++;
     }
 
     return 0;
-    
-}
+
+}

src/ros_test_pkg/srv/mySrv.srv

@@ -0,0 +1,6 @@
+#comment aasss Request message type
+int32 num_a
+int32 num_b
+---
+#comaddkdkkdkslsl Response message type
+string str