Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <robot_process/managed_robot_process.h>
00039
00040 #include <robot_process_msgs/State.h>
00041 #include <std_srvs/Empty.h>
00042
00043 namespace robot_process_tutorials
00044 {
00045
00046 class MyRobotProcess : public robot_process::ManagedRobotProcess
00047 {
00048 public:
00049
00050 using ManagedRobotProcess::ManagedRobotProcess;
00051 ~MyRobotProcess()
00052 {
00053 ROS_DEBUG_STREAM(getNamespace() << " destructed!");
00054 }
00055
00056 private:
00057 void onManagedCreate() override
00058 {
00059 subscriber_manager.subscribe("/heartbeat", 1,
00060 &MyRobotProcess::mySubscriberCallback, this);
00061
00062 service_manager.advertiseService("test",
00063 &MyRobotProcess::myServiceCallback, this);
00064
00065 registerIsolatedTimer(std::bind(&MyRobotProcess::myTimerCallback, this),
00066 0.5, true);
00067 };
00068 void onManagedTerminate() override {};
00069
00070 void onManagedConfigure() override {};
00071 void onManagedUnconfigure() override {};
00072
00073 void onManagedStart() override {};
00074 void onManagedStop() override {};
00075
00076 void onManagedResume() override {};
00077 void onManagedPause() override {};
00078
00079 void myTimerCallback()
00080 {
00081 ROS_INFO_STREAM(getNamespace() << " Timer Counter: " << counter);
00082 counter++;
00083 unsigned int seed = time(NULL);
00084 float r2 = rand_r(&seed) / (RAND_MAX / 0.10);
00085 ros::Duration(2.05 - r2).sleep();
00086 };
00087
00088 void mySubscriberCallback(boost::shared_ptr<robot_process_msgs::State const> msg)
00089 {
00090 ROS_INFO_STREAM(getNamespace() << " "
00091 << msg->node_name << " is in " << unsigned(msg->state));
00092 };
00093
00094 bool myServiceCallback(
00095 std_srvs::Empty::Request& request,
00096 std_srvs::Empty::Response& response)
00097 {
00098 ROS_INFO_STREAM(getNamespace() << " Service called, returning true");
00099 return true;
00100 };
00101
00102 int counter = 0;
00103 };
00104
00105 }
00106
00107 int main(int argc, char *argv[])
00108 {
00109 using robot_process_tutorials::MyRobotProcess;
00110
00111 MyRobotProcess node1(argc, argv, "first");
00112 node1.init().runAsync();
00113
00114 MyRobotProcess node2(argc, argv, "second");
00115 node2.init().runAsync();
00116
00117 ros::waitForShutdown();
00118 return 0;
00119 }