cartesian_controller_utils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <string>
19 #include <vector>
20 #include <algorithm>
22 
23 geometry_msgs::Pose CartesianControllerUtils::getPose(const std::string& target_frame, const std::string& source_frame)
24 {
25  geometry_msgs::Pose pose;
26  tf::StampedTransform stamped_transform;
27 
28  stamped_transform = getStampedTransform(target_frame, source_frame);
29 
30  tf::pointTFToMsg(stamped_transform.getOrigin(), pose.position);
31  tf::quaternionTFToMsg(stamped_transform.getRotation(), pose.orientation);
32 
33  return pose;
34 }
35 
36 tf::StampedTransform CartesianControllerUtils::getStampedTransform(const std::string& target_frame, const std::string& source_frame)
37 {
38  tf::StampedTransform stamped_transform;
39  bool transform = false;
40 
41  do
42  {
43  try
44  {
46  tf_listener_.waitForTransform(target_frame, source_frame, now, ros::Duration(0.1));
47  tf_listener_.lookupTransform(target_frame, source_frame, now, stamped_transform);
48  transform = true;
49  }
50  catch (tf::TransformException& ex)
51  {
52  ROS_ERROR("CartesianControllerUtils::getStampedTransform: \n%s", ex.what());
53  ros::Duration(0.1).sleep();
54  }
55  } while (!transform && ros::ok());
56 
57  return stamped_transform;
58 }
59 
60 void CartesianControllerUtils::transformPose(const std::string source_frame, const std::string target_frame, const geometry_msgs::Pose pose_in, geometry_msgs::Pose& pose_out)
61 {
62  bool transform = false;
63  geometry_msgs::PoseStamped stamped_in, stamped_out;
64  stamped_in.header.frame_id = source_frame;
65  stamped_in.pose = pose_in;
66 
67  do
68  {
69  try
70  {
72  tf_listener_.waitForTransform(target_frame, source_frame, now, ros::Duration(0.1));
73  tf_listener_.transformPose(target_frame, stamped_in, stamped_out);
74  pose_out = stamped_out.pose;
75  transform = true;
76  }
77  catch (tf::TransformException& ex)
78  {
79  ROS_ERROR("CartesianControllerUtils::transformPose: \n%s", ex.what());
80  ros::Duration(0.1).sleep();
81  }
82  } while (!transform && ros::ok());
83 }
84 
85 
89 bool CartesianControllerUtils::inEpsilonArea(const tf::StampedTransform& stamped_transform, const double epsilon)
90 {
91  double roll, pitch, yaw;
92  stamped_transform.getBasis().getRPY(roll, pitch, yaw);
93 
94  bool x_okay, y_okay, z_okay = false;
95  bool roll_okay, pitch_okay, yaw_okay = false;
96 
97  x_okay = std::fabs(stamped_transform.getOrigin().x()) < epsilon;
98  y_okay = std::fabs(stamped_transform.getOrigin().y()) < epsilon;
99  z_okay = std::fabs(stamped_transform.getOrigin().z()) < epsilon;
100 
101  roll_okay = std::fabs(roll) < epsilon;
102  pitch_okay = std::fabs(pitch) < epsilon;
103  yaw_okay = std::fabs(yaw) < epsilon;
104 
105  if (x_okay && y_okay && z_okay && roll_okay && pitch_okay && yaw_okay)
106  {
107  return true;
108  }
109  else
110  {
111  return false;
112  }
113 }
114 
115 void CartesianControllerUtils::poseToRPY(const geometry_msgs::Pose& pose, double& roll, double& pitch, double& yaw)
116 {
117  tf::Quaternion q;
118  tf::quaternionMsgToTF(pose.orientation, q);
119  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
120 }
121 
122 void CartesianControllerUtils::previewPath(const geometry_msgs::PoseArray pose_array)
123 {
124  visualization_msgs::Marker marker;
125  marker.type = visualization_msgs::Marker::SPHERE;
126  marker.lifetime = ros::Duration();
127  marker.action = visualization_msgs::Marker::ADD;
128  marker.header = pose_array.header;
129  marker.ns = "preview";
130  marker.scale.x = 0.01;
131  marker.scale.y = 0.01;
132  marker.scale.z = 0.01;
133  marker.color.r = 1.0;
134  marker.color.g = 0.0;
135  marker.color.b = 1.0;
136  marker.color.a = 1.0;
137 
138  // clear marker_array_ to only show preview of current path
139  marker_array_.markers.clear();
140 
141  double id = marker_array_.markers.size();
142 
143  for (unsigned int i = 0; i < pose_array.poses.size(); i++)
144  {
145  marker.id = id + i;
146  marker.pose = pose_array.poses.at(i);
147  marker_array_.markers.push_back(marker);
148  }
149 
151 }
152 
153 void CartesianControllerUtils::adjustArrayLength(std::vector<cob_cartesian_controller::PathArray>& m)
154 {
155  unsigned int max_steps = 0;
156  for (unsigned int i = 0; i < m.size(); i++)
157  {
158  max_steps = std::max((unsigned int)m[i].array_.size(), max_steps);
159  }
160 
161  for (unsigned int i = 0; i < m.size(); i++)
162  {
163  if (m[i].array_.size() < max_steps)
164  {
165  m[i].array_.resize(max_steps, m[i].array_.back());
166  }
167  }
168 }
169 
170 void CartesianControllerUtils::copyMatrix(std::vector<double>* path_array, std::vector<cob_cartesian_controller::PathArray>& m)
171 {
172  for (unsigned int i = 0; i < m.size(); i++)
173  {
174  path_array[i] = m[i].array_;
175  }
176 }
177 
178 double CartesianControllerUtils::roundUpToMultiplier(const double numberToRound, const double multiplier)
179 {
180  return ( multiplier - std::fmod(numberToRound, multiplier) + numberToRound );
181 }
double now()
visualization_msgs::MarkerArray marker_array_
void previewPath(const geometry_msgs::PoseArray pose_array)
void publish(const boost::shared_ptr< M > &message) const
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
bool sleep() const
tf::StampedTransform getStampedTransform(const std::string &target_frame, const std::string &source_frame)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
double roundUpToMultiplier(const double numberToRound, const double multiplier)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
void adjustArrayLength(std::vector< cob_cartesian_controller::PathArray > &m)
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool inEpsilonArea(const tf::StampedTransform &stamped_transform, const double epsilon)
tf::TransformListener tf_listener_
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
void transformPose(const std::string source_frame, const std::string target_frame, const geometry_msgs::Pose pose_in, geometry_msgs::Pose &pose_out)
geometry_msgs::Pose getPose(const std::string &target_frame, const std::string &source_frame)
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Quaternion getRotation() const
static Time now()
void poseToRPY(const geometry_msgs::Pose &pose, double &roll, double &pitch, double &yaw)
void copyMatrix(std::vector< double > *path_array, std::vector< cob_cartesian_controller::PathArray > &m)
#define ROS_ERROR(...)
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)


cob_cartesian_controller
Author(s): Christoph Mark
autogenerated on Thu Apr 8 2021 02:40:13