18 #ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H 19 #define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H 28 #include <geometry_msgs/PoseArray.h> 29 #include <visualization_msgs/MarkerArray.h> 40 void transformPose(
const std::string source_frame,
const std::string
target_frame,
const geometry_msgs::Pose pose_in, geometry_msgs::Pose& pose_out);
42 geometry_msgs::Pose
getPose(
const std::string& target_frame,
const std::string& source_frame);
45 void poseToRPY(
const geometry_msgs::Pose&
pose,
double& roll,
double& pitch,
double& yaw);
47 void previewPath(
const geometry_msgs::PoseArray pose_array);
50 void copyMatrix(std::vector<double>* path_array, std::vector<cob_cartesian_controller::PathArray>& m);
61 #endif // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H
visualization_msgs::MarkerArray marker_array_
void previewPath(const geometry_msgs::PoseArray pose_array)
tf::StampedTransform getStampedTransform(const std::string &target_frame, const std::string &source_frame)
ros::Publisher marker_pub_
double roundUpToMultiplier(const double numberToRound, const double multiplier)
void adjustArrayLength(std::vector< cob_cartesian_controller::PathArray > &m)
bool inEpsilonArea(const tf::StampedTransform &stamped_transform, const double epsilon)
tf::TransformListener tf_listener_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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)
CartesianControllerUtils()
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)