jog_frame_node.cpp
Go to the documentation of this file.
1 #include <cfloat>
3 
4 namespace jog_frame {
5 
7 {
8  ros::NodeHandle gnh;
9 
10  // Get controller information from move_group/controller_list
11  if (!gnh.hasParam("move_group/controller_list"))
12  {
13  ROS_ERROR_STREAM("move_group/controller_list is not specified.");
14  return -1;
15  }
16  XmlRpc::XmlRpcValue controller_list;
17  gnh.getParam("move_group/controller_list", controller_list);
18  for (int i = 0; i < controller_list.size(); i++)
19  {
20  if (!controller_list[i].hasMember("name"))
21  {
22  ROS_ERROR("name must be specifed for each controller.");
23  return -1;
24  }
25  if (!controller_list[i].hasMember("joints"))
26  {
27  ROS_ERROR("joints must be specifed for each controller.");
28  return -1;
29  }
30  try
31  {
32  // get name member
33  std::string name = std::string(controller_list[i]["name"]);
34  // get action_ns member if exists
35  std::string action_ns = std::string("");
36  if (controller_list[i].hasMember("action_ns"))
37  {
38  action_ns = std::string(controller_list[i]["action_ns"]);
39  }
40  // get joints member
41  if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray)
42  {
43  ROS_ERROR_STREAM("joints for controller " << name << " is not specified as an array");
44  return -1;
45  }
46  auto joints = controller_list[i]["joints"];
47  // Get type member
48  std::string type = std::string("FollowJointTrajectory");
49  if (!controller_list[i].hasMember("type"))
50  {
51  ROS_WARN_STREAM("type is not specifed for controller " << name << ", using default FollowJointTrajectory");
52  }
53  type = std::string(controller_list[i]["type"]);
54  if (type != "FollowJointTrajectory")
55  {
56  ROS_ERROR_STREAM("controller type " << type << " is not supported");
57  return -1;
58  }
59  // Create controller map
60  cinfo_map_[name].action_ns = action_ns;
61  cinfo_map_[name].joints.resize(joints.size());
62  for (int j = 0; j < cinfo_map_[name].joints.size(); ++j)
63  {
64  cinfo_map_[name].joints[j] = std::string(joints[j]);
65  }
66  }
67  catch (...)
68  {
69  ROS_ERROR_STREAM("Caught unknown exception while parsing controller information");
70  return -1;
71  }
72  }
73  return 0;
74 }
75 
77 {
78  ros::NodeHandle gnh, pnh("~");
79 
80  pnh.param<std::string>("target_link", target_link_, "link_6");
81  pnh.param<std::string>("group", group_name_);
82  pnh.param<double>("time_from_start", time_from_start_, 0.5);
83  pnh.param<bool>("use_action", use_action_, false);
84  pnh.param<bool>("intermittent", intermittent_, false);
85 
86  if (not use_action_ && intermittent_)
87  {
88  ROS_WARN("'intermittent' param should be true with 'use_action'. Assuming to use action'");
89  use_action_ = true;
90  }
91  // Exclude joint list
92  gnh.getParam("exclude_joint_names", exclude_joints_);
93 
94  if (get_controller_list() < 0)
95  {
96  ROS_ERROR("get_controller_list faild. Aborted.");
97  ros::shutdown();
98  return;
99  }
100  ROS_INFO_STREAM("controller_list:");
101  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
102  {
103  auto cinfo = it->second;
104  ROS_INFO_STREAM("- " << it->first);
105  for (int i=0; i<cinfo.joints.size(); i++)
106  {
107  ROS_INFO_STREAM(" - " << cinfo.joints[i]);
108  }
109  }
110 
111  // Create subscribers
112  joint_state_sub_ = gnh.subscribe("joint_states", 10, &JogFrameNode::joint_state_cb, this);
113  jog_frame_sub_ = gnh.subscribe("jog_frame", 10, &JogFrameNode::jog_frame_cb, this);
114  fk_client_ = gnh.serviceClient<moveit_msgs::GetPositionFK>("compute_fk");
115  ik_client_ = gnh.serviceClient<moveit_msgs::GetPositionIK>("compute_ik");
116  ros::topic::waitForMessage<sensor_msgs::JointState>("joint_states");
117 
118  if (use_action_)
119  {
120  // Create action client for each controller
121  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
122  {
123  auto controller_name = it->first;
124  auto controller_info = it->second;
125  auto action_name = controller_name + "/" + controller_info.action_ns;
126 
127  traj_clients_[controller_name] = new TrajClient(action_name, true);
128  }
129  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
130  {
131  auto controller_name = it->first;
132  auto controller_info = it->second;
133  auto action_name = controller_name + "/" + controller_info.action_ns;
134 
135  for(;;)
136  {
137  if (traj_clients_[controller_name]->waitForServer(ros::Duration(1)))
138  {
139  ROS_INFO_STREAM(action_name << " is ready.");
140  break;
141  }
142  ROS_WARN_STREAM("Waiting for " << action_name << " server...");
143  }
144  }
145  }
146  else
147  {
148  // Create publisher for each controller
149  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
150  {
151  auto controller_name = it->first;
152  traj_pubs_[controller_name] = gnh.advertise<trajectory_msgs::JointTrajectory>(controller_name + "/command", 10);
153  }
154  }
155 }
156 
161 void JogFrameNode::jog_frame_cb(jog_msgs::JogFrameConstPtr msg)
162 {
163  joint_state_.header.stamp = ros::Time::now();
164 
165  // In intermittent mode, confirm to all of the action is completed
166  if (intermittent_)
167  {
168  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
169  {
170  auto controller_name = it->first;
171  actionlib::SimpleClientGoalState state = traj_clients_[controller_name]->getState();
173  {
174  return;
175  }
176  }
177  }
178  // Update reference frame only if the stamp is older than last_stamp_ + time_from_start_
179  if (msg->header.stamp > last_stamp_ + ros::Duration(time_from_start_))
180  {
181  joint_state_.name.clear();
182  joint_state_.position.clear();
183  joint_state_.velocity.clear();
184  joint_state_.effort.clear();
185  for (auto it=joint_map_.begin(); it!=joint_map_.end(); it++)
186  {
187  // Exclude joint in exclude_joints_
188  if (std::find(exclude_joints_.begin(),
189  exclude_joints_.end(), it->first) != exclude_joints_.end())
190  {
191  ROS_INFO_STREAM("joint " << it->first << "is excluded from FK");
192  continue;
193  }
194  // Update reference joint_state
195  joint_state_.name.push_back(it->first);
196  joint_state_.position.push_back(it->second);
197  joint_state_.velocity.push_back(0.0);
198  joint_state_.effort.push_back(0.0);
199  }
200  // Update forward kinematics
201  moveit_msgs::GetPositionFK fk;
202 
203  fk.request.header.frame_id = msg->header.frame_id;
204  fk.request.header.stamp = ros::Time::now();
205  fk.request.fk_link_names.clear();
206  fk.request.fk_link_names.push_back(msg->link_name);
207  fk.request.robot_state.joint_state = joint_state_;
208 
209  if (fk_client_.call(fk))
210  {
211  if(fk.response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
212  {
213  ROS_INFO_STREAM("fk: " << fk.request);
214  ROS_WARN("****FK error %d", fk.response.error_code.val);
215  return;
216  }
217  if (fk.response.pose_stamped.size() != 1)
218  {
219  for (int i=0; i<fk.response.pose_stamped.size(); i++)
220  {
221  ROS_ERROR_STREAM("fk[" << i << "]:\n" << fk.response.pose_stamped[0]);
222  }
223  }
224  pose_stamped_ = fk.response.pose_stamped[0];
225  }
226  else
227  {
228  ROS_ERROR("Failed to call service /computte_fk");
229  return;
230  }
231  }
232  // Update timestamp of the last jog command
233  last_stamp_ = msg->header.stamp;
234 
235  // Solve inverse kinematics
236  moveit_msgs::GetPositionIK ik;
237 
238  ik.request.ik_request.group_name = msg->group_name;
239  ik.request.ik_request.ik_link_name = msg->link_name;
240  ik.request.ik_request.robot_state.joint_state = joint_state_;
241  ik.request.ik_request.avoid_collisions = msg->avoid_collisions;
242 
243  geometry_msgs::Pose act_pose = pose_stamped_.pose;
244  geometry_msgs::PoseStamped ref_pose;
245 
246  ref_pose.header.frame_id = msg->header.frame_id;
247  ref_pose.header.stamp = ros::Time::now();
248  ref_pose.pose.position.x = act_pose.position.x + msg->linear_delta.x;
249  ref_pose.pose.position.y = act_pose.position.y + msg->linear_delta.y;
250  ref_pose.pose.position.z = act_pose.position.z + msg->linear_delta.z;
251 
252  // Apply orientation jog
253  tf::Quaternion q_ref, q_act, q_jog;
254  tf::quaternionMsgToTF(act_pose.orientation, q_act);
255  double angle = sqrt(msg->angular_delta.x*msg->angular_delta.x +
256  msg->angular_delta.y*msg->angular_delta.y +
257  msg->angular_delta.z*msg->angular_delta.z);
258  tf::Vector3 axis(0,0,1);
259  if (fabs(angle) < DBL_EPSILON)
260  {
261  angle = 0.0;
262  }
263  else
264  {
265  axis.setX(msg->angular_delta.x/angle);
266  axis.setY(msg->angular_delta.y/angle);
267  axis.setZ(msg->angular_delta.z/angle);
268  }
269  //ROS_INFO_STREAM("axis: " << axis.x() << ", " << axis.y() << ", " << axis.z());
270  //ROS_INFO_STREAM("angle: " << angle);
271 
272  q_jog.setRotation(axis, angle);
273  q_ref = q_jog*q_act;
274  quaternionTFToMsg(q_ref, ref_pose.pose.orientation);
275 
276  ik.request.ik_request.pose_stamped = ref_pose;
277 
278  // ROS_INFO_STREAM("ik:\n" << ik.request);
279 
280  if (!ik_client_.call(ik))
281  {
282  ROS_ERROR("Failed to call service /compute_ik");
283  return;
284  }
285  if (ik.response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
286  {
287  ROS_WARN("****IK error %d", ik.response.error_code.val);
288  return;
289  }
290 
291  // ROS_INFO_STREAM("ik response:\n" << ik.response);
292 
293  auto state = ik.response.solution.joint_state;
294  geometry_msgs::PoseStamped pose_check;
295 
296  // Make sure the solution is valid in joint space
297  double error = 0;
298  for (int i=0; i<state.name.size(); i++)
299  {
300  for (int j=0; j<joint_state_.name.size(); j++)
301  {
302  if (state.name[i] == joint_state_.name[j])
303  {
304  double e = fabs(state.position[i] - joint_state_.position[j]);
305  if (e > error)
306  {
307  error = e;
308  }
309  break;
310  }
311  }
312  }
313  if (error > M_PI / 2)
314  {
315  ROS_ERROR_STREAM("**** Validation check Failed: " << error);
316  return;
317  }
318  // Publish trajectory message for each controller
319  for (auto it=cinfo_map_.begin(); it!=cinfo_map_.end(); it++)
320  {
321  auto controller_name = it->first;
322  auto joint_names = it->second.joints;
323 
324  std::vector<double> positions, velocities, accelerations;
325 
326  positions.resize(joint_names.size());
327  velocities.resize(joint_names.size());
328  accelerations.resize(joint_names.size());
329 
330  for (int i=0; i<joint_names.size(); i++)
331  {
332  size_t index = std::distance(state.name.begin(),
333  std::find(state.name.begin(),
334  state.name.end(), joint_names[i]));
335  if (index == state.name.size())
336  {
337  ROS_WARN_STREAM("Cannot find joint " << joint_names[i] << " in IK solution");
338  }
339  positions[i] = state.position[index];
340  velocities[i] = 0;
341  accelerations[i] = 0;
342  }
343  trajectory_msgs::JointTrajectoryPoint point;
344  point.positions = positions;
345  point.velocities = velocities;
346  point.accelerations = accelerations;
347  point.time_from_start = ros::Duration(time_from_start_);
348 
349  if (use_action_)
350  {
351  control_msgs::FollowJointTrajectoryGoal goal;
352  goal.trajectory.header.stamp = ros::Time::now();
353  goal.trajectory.header.frame_id = "base_link";
354  goal.trajectory.joint_names = joint_names;
355  goal.trajectory.points.push_back(point);
356 
357  traj_clients_[controller_name]->sendGoal(goal);
358  }
359  else
360  {
361  trajectory_msgs::JointTrajectory traj;
362  traj.header.stamp = ros::Time::now();
363  traj.header.frame_id = "base_link";
364  traj.joint_names = joint_names;
365  traj.points.push_back(point);
366 
367  traj_pubs_[controller_name].publish(traj);
368  }
369  }
370  // update pose_stamped_
371  pose_stamped_.pose = ref_pose.pose;
372 
373 }
374 
379 void JogFrameNode::joint_state_cb(sensor_msgs::JointStateConstPtr msg)
380 {
381  // Check that the msg contains joints
382  if (msg->name.empty() || msg->name.size() != msg->position.size())
383  {
384  ROS_WARN("Invalid JointState message");
385  return;
386  }
387  // Update joint information
388  for (int i=0; i<msg->name.size(); i++)
389  {
390  joint_map_[msg->name[i]] = msg->position[i];
391  }
392 }
393 
394 } // namespace jog_arm
395 
399 int main(int argc, char **argv)
400 {
401  ros::init(argc, argv, "jog_frame_node");
403 
404  ros::Rate loop_rate(10);
405  while ( ros::ok() )
406  {
407  ros::spinOnce();
408  loop_rate.sleep();
409  }
410  return 0;
411 }
412 
actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::ServiceClient ik_client_
ros::Subscriber jog_frame_sub_
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
std::map< std::string, double > joint_map_
void jog_frame_cb(jog_msgs::JogFrameConstPtr msg)
Callback function for the topic jog_frame.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
bool call(MReq &req, MRes &res)
geometry_msgs::PoseStamped pose_stamped_
void joint_state_cb(sensor_msgs::JointStateConstPtr msg)
Callback function for the topic joint_state.
#define M_PI
#define ROS_WARN(...)
int main(int argc, char **argv)
Main function of the node.
unsigned int index
ros::ServiceClient fk_client_
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
std::map< std::string, Controller > cinfo_map_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Subscriber joint_state_sub_
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
#define ROS_WARN_STREAM(args)
bool sleep()
std::map< std::string, ros::Publisher > traj_pubs_
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
static Time now()
ROSCPP_DECL void shutdown()
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
void setRotation(const Vector3 &axis, const tfScalar &angle)
std::vector< std::string > exclude_joints_
#define ROS_ERROR(...)
sensor_msgs::JointState joint_state_
std::map< std::string, TrajClient * > traj_clients_


jog_controller
Author(s): Ryosuke Tajima
autogenerated on Sun May 17 2020 03:25:01