multi_robot_mobile.cpp
Go to the documentation of this file.
1 /*
2  * [c++ example] multi robot on mobile
3  * Author: Kab Kyoum Kim (kabkyoum.kim@doosan.com)
4  *
5  * Copyright (c) 2019 Doosan Robotics
6  * Use of this source code is governed by the BSD, see LICENSE
7 */
8 
9 #include <ros/ros.h>
10 #include <geometry_msgs/Twist.h>
11 #include <stdlib.h>
12 #include <signal.h>
13 
14 #include "dsr_util.h"
15 #include "dsr_robot.h"
16 
17 using namespace std;
18 using namespace DSR_Robot;
19 
20 //----- set tartget robot----------------------------------------------------
21 string ROBOT_ID = "dsr01";
22 string ROBOT_ID2 = "dsr02";
23 string ROBOT_MODEL = "m1013";
24 string ROBOT_MODEL2 = "m1013";
25 void SET_ROBOT (string id, string model) {ROBOT_ID = id; ROBOT_MODEL = model;}
26 void SET_ROBOT2(string id, string model) {ROBOT_ID2= id; ROBOT_MODEL2= model;}
27 //---------------------------------------------------------------------------
28 
30 
31 static void thread_mobile()
32 {
33  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
34  ros::Publisher pubMobile = node->advertise<geometry_msgs::Twist>("/"+ROBOT_ID+"/twist_marker_server/cmd_vel", 1000);
35 
36  srand(time(0));
37  geometry_msgs::Twist msg;
38 
39  while(ros::ok())
40  {
41  msg.linear.x = double(rand())/double(RAND_MAX);
42  msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;
43  pubMobile.publish(msg);
44 
45  //ROS_INFO("thread_mobile running...");
46  time_sleep(0.1);
47  }
48 
49 }
50 static void thread_mobile2()
51 {
52  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
53  ros::Publisher pubMobile2 = node->advertise<geometry_msgs::Twist>("/"+ROBOT_ID2+"/twist_marker_server/cmd_vel", 1000);
54 
55  srand(time(0));
56  geometry_msgs::Twist msg;
57 
58  while(ros::ok())
59  {
60  msg.linear.x = double(rand())/double(RAND_MAX);
61  msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;
62  pubMobile2.publish(msg);
63 
64  //ROS_INFO("thread_mobile2 running...");
65  time_sleep(0.1);
66  }
67 
68 }
69 
70 void SigHandler(int sig)
71 {
72  // Do some custom action.
73  // For example, publish a stop message to some other nodes.
74 
75  // All the default sigint handler does is call shutdown()
76  ROS_INFO("shutdown time! sig=%d",sig);
77  ROS_INFO("shutdown time! sig=%d",sig);
78  ROS_INFO("shutdown time! sig=%d",sig);
79  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/dsr01m1013/motion/move_stop");
80 
81  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
82  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",100);
83  ros::Publisher pubRobotStop2= node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2+ROBOT_MODEL+"/stop",100);
84 
85  dsr_msgs::RobotStop msg;
86 
87  msg.stop_mode = STOP_TYPE_QUICK;
88  pubRobotStop.publish(msg);
89  pubRobotStop2.publish(msg);
90 
91  ros::shutdown();
92 }
93 
94 int main(int argc, char** argv)
95 {
96  ros::init(argc, argv, "multi_robot_mobile_cpp", ros::init_options::NoSigintHandler);
97  ros::NodeHandle nh("~");
98  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",10);
99  ros::Publisher pubRobotStop2= nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2+ROBOT_MODEL+"/stop",10);
100 
102  ros::Publisher pubMobile = nh.advertise<geometry_msgs::Twist>("/"+ROBOT_ID +"/twist_marker_server/cmd_vel", 1000);
103  ros::Publisher pubMobile2= nh.advertise<geometry_msgs::Twist>("/"+ROBOT_ID2+"/twist_marker_server/cmd_vel", 1000);
104 
105  //----- set target robot ---------------
106  const string my_robot_id = "dsr01";
107  const string my_robot_id2 = "dsr02";
108  const string my_robot_model = "m1013";
109  const string my_robot_model2= "m1013";
110 
111  SET_ROBOT (my_robot_id, my_robot_model);
112  SET_ROBOT2(my_robot_id2, my_robot_model2);
113 
114  CDsrRobot robot (nh, my_robot_id, my_robot_model);
115  CDsrRobot robot2(nh, my_robot_id2, my_robot_model2);
116 
117  // run subscriber thread (for monitoring)
118  boost::thread thread_sub(thread_mobile);
119  boost::thread thread_sub2(thread_mobile2);
120 
121 
122  float p1[6]={0.0,}; //joint
123  float p2[6]={0.0, 0.0, 90.0, 0.0, 90.0, 0.0}; //joint
124 
125  float x1[6]={400, 500, 800.0, 0.0, 180.0, 0.0}; //task
126  float x2[6]={400, 500, 500.0, 0.0, 180.0, 0.0}; //task
127  float velx[2]={50, 50}; // 태스크 속도를 50(mm/sec), 50(deg/sec)로 설정
128  float accx[2]={100, 100}; // 태스크 가속도를 100(mm/sec2), 100(deg/sec2)로 설정
129 
130 
131  float veljx[6] = {50, 50, 50, 50, 50, 50};
132  float accjx[6] = {100, 100, 100, 100, 100, 100};
133  float fCirclePos[2][6] = {{559,434.5,651.5,0,180,0}, {559,434.5,251.5,0,180,0}};
134 
135  float amp[6] = {10,0,0,0,30,0};
136  float periodic[6] = {1,0,1.5,0,0,0};
137 
138  float fSJPos[3][6] = {{10, -10, 20, -30, 10, 20}, {25, 0, 10, -50, 20, 40}, {50, 50, 50, 50, 50, 50}};
139  float fSXPos[3][6] = {{600, 600, 600, 0, 175, 0},{600, 750, 600, 0, 175, 0},{150, 600, 450, 0, 175, 0}};
140 
141  float bx1[2][6] = {{370, 670, 650, 0, 180, 0}, {370, 670, 650, 0, 180, 0}};
142  float bx2[2][6] = {{370, 670, 400, 0, 180, 0},{370, 545, 400, 0, 180, 0}};
143  float bx3[2][6] = {{370, 595, 400, 0, 180, 0}, {370, 595, 400, 0, 180, 0}};
144 
145  MOVE_POSB posb[3];// = {bx1, 0, 40}, {bx2, 1, 40}, {bx3, 1, 40};
146  for(int i=0; i<2; i++){
147  for(int j=0; j<6; j++){
148  posb[0]._fTargetPos[i][j] = bx1[i][j];
149  }
150  }
151  for(int i=0; i<2; i++){
152  for(int j=0; j<6; j++){
153  posb[1]._fTargetPos[i][j] = bx2[i][j];
154  }
155  }
156  for(int i=0; i<2; i++){
157  for(int j=0; j<6; j++){
158  posb[2]._fTargetPos[i][j] = bx3[i][j];
159  }
160  }
161  posb[0]._iBlendType = 0;
162  posb[1]._iBlendType = 1;
163  posb[2]._iBlendType = 0;
164  posb[0]._fBlendRad = 20;
165  posb[1]._fBlendRad = 20;
166  posb[2]._fBlendRad = 20;
167 
168  while(ros::ok())
169  {
170  robot.amovej(p1, 30, 30);
171  robot2.movej(p1, 30, 30);
172 
173  robot.amovej(p2, 30, 30);
174  robot2.movej(p2, 30, 30);
175 
176  robot.amovel(x1, velx, accx);
177  robot2.movel(x1, velx, accx);
178 
179  robot.amovel(x2, velx, accx);
180  robot2.movel(x2, velx, accx);
181 
182  robot.amovec(fCirclePos, velx, accx);
183  robot2.movec(fCirclePos, velx, accx);
184 
185  robot.amove_periodic(amp, periodic, 0.5, 3, 0);
186  robot2.move_periodic(amp, periodic, 0.5, 3, 0);
187 
188  robot.amove_spiral(9.5, 20.0, 50.0, velx, accx, 20.0, TASK_AXIS_Z);
189  robot2.move_spiral(9.5, 20.0, 50.0, velx, accx, 20.0, TASK_AXIS_Z);
190 
191  robot.amovesj(fSJPos,3, 30, 100);
192  robot2.movesj(fSJPos,3, 30, 100);
193 
194  robot.amovesx(fSXPos, 3, velx, accx);
195  robot2.movesx(fSXPos, 3, velx, accx);
196 
197  robot.amoveb(posb, 3, velx, accx);
198  robot2.moveb(posb, 3, velx, accx);
199  }
200 
201  ROS_INFO("multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
202  ROS_INFO("multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
203  ROS_INFO("multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
204 
206  thread_sub.join();
207  thread_sub2.join();
208 
209  ROS_INFO("multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
210  return 0;
211 }
static void thread_mobile()
float amp[6]
float velx[2]
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
string ROBOT_ID2
void SigHandler(int sig)
float accx[2]
#define ROS_INFO(...)
string ROBOT_MODEL2
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static void thread_mobile2()
int main(int argc, char **argv)
void SET_ROBOT2(string id, string model)
void SET_ROBOT(string id, string model)
string ROBOT_ID
ROSCPP_DECL void shutdown()
string ROBOT_MODEL


cpp
Author(s): Kab Kyoum Kim , Jin Hyuk Gong , Jeongwoo Lee
autogenerated on Sat May 18 2019 02:32:53