这里记录一下cartographer里面的路标点是如何转换的,首先你要知道路标点在相机坐标系的位置(很多二维码读码器的关系恰好相反,是相机在二维码的坐标系),然后要知道相机坐标系和小车坐标系(一般为base_link)之间的关系。中间用tf_broadcaster_发出来便于查看是否设置正确
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = ros::Time::now();
stamped_transform.header.frame_id = "base_link";
stamped_transform.child_frame_id = "QR_Code";
stamped_transform.transform = ToGeometryMsgTransform(ToRigid3d(msg->landmarks[0].tracking_from_landmark_transform).inverse());
tf_broadcaster_.sendTransform(stamped_transform);