增加action,dynamic_reconfigure测试代码

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
@@ -59,16 +62,16 @@ add_service_files(
 )
 
 ## 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
 )
 
 ################################################
@@ -86,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 ##
@@ -139,6 +141,9 @@ 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
@@ -152,6 +157,12 @@ add_dependencies(subscribe_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXP
 
 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}
@@ -170,6 +181,13 @@ target_link_libraries(client_node
 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/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/start_turtle.launch

@@ -1,5 +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/control_turtle.cpp

@@ -1,35 +1,82 @@
 #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;
 
-    string cmd_topic = "/turtle1/cmd_vel";
-    double linear_x = 1.0;
-    double linear_y = 1.0;
-    double angular_z = 1.0;
-    int loop_rate = 1;
-
     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);
 
-    ros::Publisher cmdVelPub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+    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;
 
-    ros::Rate rate(loop_rate);
     while(ros::ok())
     {
-        speed.linear.x  = linear_x;
-        speed.linear.y  = linear_y;
-        speed.angular.z = angular_z;
-        cmdVelPub.publish(speed);
+        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();
     }