xarm_simple_planner_test2.cpp
Go to the documentation of this file.
1 /* Copyright 2018 UFACTORY Inc. All Rights Reserved.
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Author: Jason Peng <jason@ufactory.cc>
6  ============================================================================*/
7 #include "ros/ros.h"
8 #include <std_msgs/Bool.h>
9 #include <xarm_planner/pose_plan.h>
10 #include <xarm_planner/joint_plan.h>
11 #include <xarm_planner/exec_plan.h>
12 #include <xarm_planner/single_straight_plan.h>
13 #include <stdlib.h>
14 #include <vector>
15 
16 bool request_plan(ros::ServiceClient& client, xarm_planner::joint_plan& srv)
17 {
18  if(client.call(srv))
19  {
20  return srv.response.success;
21  }
22  else
23  {
24  ROS_ERROR("Failed to call service joint_plan");
25  return false;
26  }
27 }
28 
29 bool request_plan(ros::ServiceClient& client, xarm_planner::pose_plan& srv)
30 {
31  if(client.call(srv))
32  {
33  return srv.response.success;
34  }
35  else
36  {
37  ROS_ERROR("Failed to call service pose_plan");
38  return false;
39  }
40 }
41 
42 bool request_plan(ros::ServiceClient& client, xarm_planner::single_straight_plan& srv)
43 {
44  if(client.call(srv))
45  {
46  return srv.response.success;
47  }
48  else
49  {
50  ROS_ERROR("Failed to call service single_straight_plan");
51  return false;
52  }
53 }
54 
55 bool request_exec(ros::ServiceClient& client, xarm_planner::exec_plan& srv)
56 {
57  if(client.call(srv))
58  {
59  return srv.response.success;
60  }
61  else
62  {
63  ROS_ERROR("Failed to call service exec_plan");
64  return false;
65  }
66 }
67 
68 
69 int main(int argc, char** argv)
70 {
71  std::vector<double> tar_joint1 = {-1.0, -0.75, 0.0, -0.5, 0.0, 0.3, 0.0};
72  std::vector<double> tar_joint2 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
73  std::vector<double> tar_joint3 = {1.0, -0.75, 0.0, -0.5, 0.0, -0.3, 0.0};
74 
75  ros::init(argc, argv, "xarm_simple_planner_client");
76  ros::NodeHandle nh;
77  ros::ServiceClient client = nh.serviceClient<xarm_planner::joint_plan>("xarm_joint_plan");
78  ros::ServiceClient client2 = nh.serviceClient<xarm_planner::pose_plan>("xarm_pose_plan");
79  ros::ServiceClient client_exec = nh.serviceClient<xarm_planner::exec_plan>("xarm_exec_plan");
80 
81  ros::Publisher exec_pub = nh.advertise<std_msgs::Bool>("xarm_planner_exec", 10);
82  std_msgs::Bool msg;
83  xarm_planner::joint_plan srv;
84  xarm_planner::pose_plan srv2;
85  xarm_planner::exec_plan srv_exec;
86 
87 
88 
89  //********************************************************************
90  ros::ServiceClient client22 = nh.serviceClient<xarm_planner::pose_plan>("xarm_straight_plan");
91  std::vector<double> tar_joint22 = {-19.9/57.3, 16.9/57.3, 0.8/57.3, 21.5/57.3, -2.9/57.3, 4.6/57.3, -16.3/57.3};
92  xarm_planner::single_straight_plan srv22;
93 
94  double slp_t = 0.5;
95  geometry_msgs::Pose target1;
96  target1.position.x = 0.3;
97  target1.position.y = -0.1;
98  target1.position.z = 0.2;
99 
100  target1.orientation.x = 1;
101  target1.orientation.y = 0;
102  target1.orientation.z = 0;
103  target1.orientation.w = 0;
104 
105  geometry_msgs::Pose target2;
106  target2.position.x = 0.3;
107  target2.position.y = 0.1;
108  target2.position.z = 0.2;
109 
110  target2.orientation.x = 1;
111  target2.orientation.y = 0;
112  target2.orientation.z = 0;
113  target2.orientation.w = 0;
114 
115  geometry_msgs::Pose target3;
116  target3.position.x = 0.3;
117  target3.position.y = 0.1;
118  target3.position.z = 0.4;
119 
120  target3.orientation.x = 1;
121  target3.orientation.y = 0;
122  target3.orientation.z = 0;
123  target3.orientation.w = 0;
124 
125  geometry_msgs::Pose target4;
126  target4.position.x = 0.3;
127  target4.position.y = -0.1;
128  target4.position.z = 0.4;
129 
130  target4.orientation.x = 1;
131  target4.orientation.y = 0;
132  target4.orientation.z = 0;
133  target4.orientation.w = 0;
134 
135 
136  srv.request.target = tar_joint22;
137  if(request_plan(client, srv))
138  {
139  ROS_INFO("Plan SUCCESS! Executing... ");
140  // msg.data = true;
141  // ros::Duration(1.0).sleep();
142  // exec_pub.publish(msg);
143  srv_exec.request.exec = true;
144  request_exec(client_exec, srv_exec);
145  }
146  ros::Duration(slp_t).sleep(); // Wait for last execution to finish
147 
148  int i=0;
149  for(i=0; i<10; i++)
150  {
151  srv22.request.target = target1;
152  if(request_plan(client22, srv22))
153  {
154  ROS_INFO("Plan SUCCESS! Executing... ");
155  // msg.data = true;
156  // ros::Duration(1.0).sleep();
157  // exec_pub.publish(msg);
158  srv_exec.request.exec = true;
159  request_exec(client_exec, srv_exec);
160  }
161  else
162  break;
163 
164  srv22.request.target = target2;
165  if(request_plan(client22, srv22))
166  {
167  ROS_INFO("Plan SUCCESS! Executing... ");
168  // msg.data = true;
169  // ros::Duration(1.0).sleep();
170  // exec_pub.publish(msg);
171  srv_exec.request.exec = true;
172  request_exec(client_exec, srv_exec);
173  }
174  else
175  break;
176  srv22.request.target = target3;
177  if(request_plan(client22, srv22))
178  {
179  ROS_INFO("Plan SUCCESS! Executing... ");
180  // msg.data = true;
181  // ros::Duration(1.0).sleep();
182  // exec_pub.publish(msg);
183  srv_exec.request.exec = true;
184  request_exec(client_exec, srv_exec);
185  }
186  else
187  break;
188  srv22.request.target = target4;
189  if(request_plan(client22, srv22))
190  {
191  ROS_INFO("Plan SUCCESS! Executing... ");
192  // msg.data = true;
193  // ros::Duration(1.0).sleep();
194  // exec_pub.publish(msg);
195  srv_exec.request.exec = true;
196  request_exec(client_exec, srv_exec);
197  }
198  else
199  break;
200 
201  }
202 
203  if(i<10)
204  ROS_ERROR("Execution Failed at loop: %d!", i+1);
205 
206  // ros::Duration(4.0).sleep(); // Wait for last execution to finish
207 
208 
209  //********************************************************************
210 
211 
212  // geometry_msgs::Pose target1;
213  // target1.position.x = 0.28;
214  // target1.position.y = 0.2;
215  // target1.position.z = 0.2;
216 
217  // target1.orientation.x = 1;
218  // target1.orientation.y = 0;
219  // target1.orientation.z = 0;
220  // target1.orientation.w = 0;
221 
222 
223  // srv2.request.target = target1;
224  // if(request_plan(client2, srv2))
225  // {
226  // ROS_INFO("Plan SUCCESS! Executing... ");
227  // // msg.data = true;
228  // // ros::Duration(1.0).sleep();
229  // // exec_pub.publish(msg);
230  // srv_exec.request.exec = true;
231  // request_exec(client_exec, srv_exec);
232  // }
233 
234  // // ros::Duration(4.0).sleep(); // Wait for last execution to finish
235 
236 
237 
238  // srv.request.target = tar_joint2;
239  // if(request_plan(client, srv))
240  // {
241  // ROS_INFO("Plan SUCCESS! Executing... ");
242  // // msg.data = true;
243  // // ros::Duration(1.0).sleep();
244  // // exec_pub.publish(msg);
245  // srv_exec.request.exec = true;
246  // request_exec(client_exec, srv_exec);
247  // }
248 
249  // // ros::Duration(4.0).sleep(); // Wait for last execution to finish
250 
251  // srv2.request.target = target1;
252  // if(request_plan(client2, srv2))
253  // {
254  // ROS_INFO("Plan SUCCESS! Executing... ");
255  // // msg.data = true;
256  // // ros::Duration(1.0).sleep();
257  // // exec_pub.publish(msg);
258  // srv_exec.request.exec = true;
259  // request_exec(client_exec, srv_exec);
260  // }
261 
262  // // ros::Duration(4.0).sleep(); // Wait for last execution to finish
263 
264  // srv.request.target = tar_joint2;
265  // if(request_plan(client, srv))
266  // {
267  // ROS_INFO("Plan SUCCESS! Executing... ");
268  // // msg.data = true;
269  // // ros::Duration(1.0).sleep();
270  // // exec_pub.publish(msg);
271  // srv_exec.request.exec = true;
272  // request_exec(client_exec, srv_exec);
273  // }
274 
275  return 0;
276 
277 }
278 
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::NodeHandle
main
int main(int argc, char **argv)
Definition: xarm_simple_planner_test2.cpp:69
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::ServiceClient::call
bool call(MReq &req, MRes &res)
request_exec
bool request_exec(ros::ServiceClient &client, xarm_planner::exec_plan &srv)
Definition: xarm_simple_planner_test2.cpp:55
request_plan
bool request_plan(ros::ServiceClient &client, xarm_planner::joint_plan &srv)
Definition: xarm_simple_planner_test2.cpp:16
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROS_ERROR
#define ROS_ERROR(...)

xarm_planner
Author(s):
autogenerated on Sat May 8 2021 02:51:52