10 #include <geometry_msgs/Twist.h> 15 #include "dsr_robot.h" 34 ros::Publisher pubMobile = node->advertise<geometry_msgs::Twist>(
"/"+
ROBOT_ID+
"/twist_marker_server/cmd_vel", 1000);
37 geometry_msgs::Twist msg;
41 msg.linear.x = double(rand())/double(RAND_MAX);
42 msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;
43 pubMobile.publish(msg);
53 ros::Publisher pubMobile2 = node->advertise<geometry_msgs::Twist>(
"/"+
ROBOT_ID2+
"/twist_marker_server/cmd_vel", 1000);
56 geometry_msgs::Twist msg;
60 msg.linear.x = double(rand())/double(RAND_MAX);
61 msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;
62 pubMobile2.publish(msg);
76 ROS_INFO(
"shutdown time! sig=%d",sig);
77 ROS_INFO(
"shutdown time! sig=%d",sig);
78 ROS_INFO(
"shutdown time! sig=%d",sig);
85 dsr_msgs::RobotStop msg;
87 msg.stop_mode = STOP_TYPE_QUICK;
88 pubRobotStop.publish(msg);
89 pubRobotStop2.publish(msg);
94 int main(
int argc,
char** argv)
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";
114 CDsrRobot robot (nh, my_robot_id, my_robot_model);
115 CDsrRobot robot2(nh, my_robot_id2, my_robot_model2);
123 float p2[6]={0.0, 0.0, 90.0, 0.0, 90.0, 0.0};
125 float x1[6]={400, 500, 800.0, 0.0, 180.0, 0.0};
126 float x2[6]={400, 500, 500.0, 0.0, 180.0, 0.0};
127 float velx[2]={50, 50};
128 float accx[2]={100, 100};
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}};
135 float amp[6] = {10,0,0,0,30,0};
136 float periodic[6] = {1,0,1.5,0,0,0};
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}};
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}};
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];
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];
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];
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;
170 robot.amovej(p1, 30, 30);
171 robot2.movej(p1, 30, 30);
173 robot.amovej(p2, 30, 30);
174 robot2.movej(p2, 30, 30);
176 robot.amovel(x1, velx, accx);
177 robot2.movel(x1, velx, accx);
179 robot.amovel(x2, velx, accx);
180 robot2.movel(x2, velx, accx);
182 robot.amovec(fCirclePos, velx, accx);
183 robot2.movec(fCirclePos, velx, accx);
185 robot.amove_periodic(amp, periodic, 0.5, 3, 0);
186 robot2.move_periodic(amp, periodic, 0.5, 3, 0);
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);
191 robot.amovesj(fSJPos,3, 30, 100);
192 robot2.movesj(fSJPos,3, 30, 100);
194 robot.amovesx(fSXPos, 3, velx, accx);
195 robot2.movesx(fSXPos, 3, velx, accx);
197 robot.amoveb(posb, 3, velx, accx);
198 robot2.moveb(posb, 3, velx, accx);
201 ROS_INFO(
"multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
202 ROS_INFO(
"multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
203 ROS_INFO(
"multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
209 ROS_INFO(
"multi_robot_mobile_cpp finished !!!!!!!!!!!!!!!!!!!!!");
static void thread_mobile()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
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)
ROSCPP_DECL void shutdown()