tftf2

tf/tf2

矩阵换算

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"//tf2 转变坐标系
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf2_;geometry_msgs::PointStamped point_out;
buffer_.transform(pose_in, pose_out, target_frame_);// 方式2tf2::doTransform(pose_in, pose_out, geometry_msgs::TransformStamped tf_plan_to_global);//tf
try {listener_.waitForTransform(odom_frame_, "base_link", ros::Time(0), ros::Duration(0.1));listener_.transformPose("base_link", pose_in_odom, pose_in_base);
} catch (tf::TransformException const& ex) {ROS_WARN("BaseController, Couldn't transform dock pose to tracking frame");return false;
}

diff2D, 获得输出速度PoseFollower.cpp

// usage
// diff2D(target_pose, robot_pose);geometry_msgs::Twist diff2D(const tf::Pose& pose1, const tf::Pose& pose2){geometry_msgs::Twist res;tf::Pose diff = pose2.inverse() * pose1;res.linear.x = diff.getOrigin().x();res.linear.y = diff.getOrigin().y();res.angular.z = tf::getYaw(diff.getRotation());if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))return res;//in the case that we're not rotating to our goal position and we have a non-holonomic robot//we'll need to command a rotational velocity that will help us reach our desired heading//we want to compute a goal based on the heading difference between our pose and the targetdouble yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));//compute the desired quateriontf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);tf::Quaternion rot = pose2.getRotation() * rot_diff;tf::Pose new_pose = pose1;new_pose.setRotation(rot);diff = pose2.inverse() * new_pose;res.linear.x = diff.getOrigin().x();res.linear.y = diff.getOrigin().y();res.angular.z = tf::getYaw(diff.getRotation());return res;}// 计算yawdiff， PoseFollower.cpp
// x,y 是 target， pt 是current pose;  这些点都是在odom 下讲的double headingDiff(double x, double y, double pt_x, double pt_y, double heading){double v1_x = x - pt_x;double v1_y = y - pt_y;double v2_x = cos(heading);double v2_y = sin(heading);double perp_dot = v1_x * v2_y - v1_y * v2_x;   // 叉乘的意思（法线）， |a| * |b| * sindouble dot = v1_x * v2_x + v1_y * v2_y;        // 点乘的意义(投影)， |a| × |b| × cos//get the signed angledouble vector_angle = atan2(perp_dot, dot);return -1.0 * vector_angle;}

点乘叉乘