提交launch相关代码、自定义服务通信代码

src/ros_test_pkg/CMakeLists.txt

@@ -53,11 +53,10 @@ 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(
@@ -136,6 +135,10 @@ 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)
 ## 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,8 +147,11 @@ 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})
 ## Specify libraries to link a library or executable target against
 target_link_libraries(publish_node
   ${catkin_LIBRARIES}
@@ -154,6 +160,16 @@ 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}
+)
 #############
 ## Install ##
 #############

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/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,5 @@
+<launch>
+    <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>
+</launch>

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,38 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+using namespace std;
+
+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);
+    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);
+
+        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