25 geometry_msgs::Pose
pose;
39 bool transform =
false;
52 ROS_ERROR(
"CartesianControllerUtils::getStampedTransform: \n%s", ex.what());
55 }
while (!transform &&
ros::ok());
57 return stamped_transform;
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;
74 pose_out = stamped_out.pose;
79 ROS_ERROR(
"CartesianControllerUtils::transformPose: \n%s", ex.what());
82 }
while (!transform &&
ros::ok());
91 double roll, pitch, yaw;
94 bool x_okay, y_okay, z_okay =
false;
95 bool roll_okay, pitch_okay, yaw_okay =
false;
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;
101 roll_okay = std::fabs(roll) < epsilon;
102 pitch_okay = std::fabs(pitch) < epsilon;
103 yaw_okay = std::fabs(yaw) < epsilon;
105 if (x_okay && y_okay && z_okay && roll_okay && pitch_okay && yaw_okay)
124 visualization_msgs::Marker marker;
125 marker.type = visualization_msgs::Marker::SPHERE;
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;
143 for (
unsigned int i = 0; i < pose_array.poses.size(); i++)
146 marker.pose = pose_array.poses.at(i);
155 unsigned int max_steps = 0;
156 for (
unsigned int i = 0; i < m.size(); i++)
158 max_steps = std::max((
unsigned int)m[i].array_.size(), max_steps);
161 for (
unsigned int i = 0; i < m.size(); i++)
163 if (m[i].array_.size() < max_steps)
165 m[i].array_.resize(max_steps, m[i].array_.back());
172 for (
unsigned int i = 0; i < m.size(); i++)
174 path_array[i] = m[i].array_;
180 return ( multiplier - std::fmod(numberToRound, multiplier) + numberToRound );
visualization_msgs::MarkerArray marker_array_
void previewPath(const geometry_msgs::PoseArray pose_array)
void publish(const boost::shared_ptr< M > &message) const
tf::StampedTransform getStampedTransform(const std::string &target_frame, const std::string &source_frame)
ros::Publisher marker_pub_
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)
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool inEpsilonArea(const tf::StampedTransform &stamped_transform, const double epsilon)
tf::TransformListener tf_listener_
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 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)
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)