catalogue
The file structure in the project is as follows:
4. Preparation of launch document:
5. The operation results are as follows:
The file structure in the project is as follows:
To realize coordinate tracking, first clarify the logic of code writing:
1. Start the turnle1 node and the keyboard operation node of turnle1, subscribe to the pose information of turnle1, and publish the coordinate information of turnle1 (getturnle1pose. CPP):
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void SubCallbackFunc(const turtlesim::Pose::ConstPtr& TurtlePoseInfo)
{
//The "static attribute" ensures that the publisher object remains unchanged
static tf2_ros::StaticTransformBroadcaster pub;
geometry_msgs::TransformStamped TransformInfo;
tf2::Quaternion qtn;
TransformInfo.child_frame_id = "Turtle1Frame";
TransformInfo.header.frame_id = "world";
TransformInfo.header.stamp = ros::Time::now();
TransformInfo.transform.translation.x = TurtlePoseInfo->x;
TransformInfo.transform.translation.y = TurtlePoseInfo->y;
qtn.setRPY(0,0,TurtlePoseInfo->theta);
TransformInfo.transform.rotation.w = qtn.getW();
TransformInfo.transform.rotation.x = qtn.getX();
TransformInfo.transform.rotation.y = qtn.getY();
TransformInfo.transform.rotation.z = qtn.getZ();
pub.sendTransform(TransformInfo);
}
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"getTurtle1Pose");
ros::NodeHandle nh("turtle1");
//Subscribe to the posture of turnle1 (subscribe to messages through private topics)
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("pose",10,boost::bind(SubCallbackFunc,_1));
ros::spin();
return 0;
} be careful:
1) In order to ensure that the publisher object that publishes the turtle1 coordinate information is constant, we declare the publishing object of the turtle1 coordinate information as static in the function, which means that the publishing object is created only once, which prevents the "disadvantage of frequent creation of publisher objects subscribing to tf_static topics";
2) The structure of the code is as follows:
3) The code implementation logic is as follows:
2. Start the turtle 2 node, subscribe to the pose information of the turtle 2, and publish the coordinate information of the turtle 2 (getturtle 2pose. CPP):
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void SubCallbackFunc(const turtlesim::Pose::ConstPtr& TurtlePoseInfo)
{
//The "static attribute" ensures that the publisher object remains unchanged
static tf2_ros::StaticTransformBroadcaster pub;
geometry_msgs::TransformStamped TransformInfo;
tf2::Quaternion qtn;
TransformInfo.child_frame_id = "Turtle1Frame";
TransformInfo.header.frame_id = "world";
TransformInfo.header.stamp = ros::Time::now();
TransformInfo.transform.translation.x = TurtlePoseInfo->x;
TransformInfo.transform.translation.y = TurtlePoseInfo->y;
qtn.setRPY(0,0,TurtlePoseInfo->theta);
TransformInfo.transform.rotation.w = qtn.getW();
TransformInfo.transform.rotation.x = qtn.getX();
TransformInfo.transform.rotation.y = qtn.getY();
TransformInfo.transform.rotation.z = qtn.getZ();
pub.sendTransform(TransformInfo);
}
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"getTurtle1Pose");
ros::NodeHandle nh("turtle1");
//Subscribe to the posture of turnle1 (subscribe to messages through private topics)
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("pose",10,boost::bind(SubCallbackFunc,_1));
ros::spin();
return 0;
} be careful:
1) The structure of the code is as follows:
2) The implementation logic of the code is as follows:
3. Subscribe / tf_static topic, obtain the relative relationship of coordinate system and publish the motion information of turnle2 (frameTransform.cpp):
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "math.h"
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"frameTrasnform");
//Create a buffer dedicated to the listener
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
//Create CMD_ Publisher of vel topic
geometry_msgs::Twist ExpectedRunState;
ros::NodeHandle nh("turtle2");
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
ros::Rate rate(1);
while(ros::ok())
{
try
{
geometry_msgs::TransformStamped Turtle1ToTurtle2 = buffer.lookupTransform("Turtle2Frame","Turtle1Frame",ros::Time());
ExpectedRunState.angular.z = std::atan2(Turtle1ToTurtle2.transform.rotation.y,Turtle1ToTurtle2.transform.rotation.x);
ExpectedRunState.linear.x = Turtle1ToTurtle2.transform.translation.x;
ExpectedRunState.linear.y = Turtle1ToTurtle2.transform.translation.y;
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
pub.publish(ExpectedRunState);
rate.sleep();
}
}be careful:
1) The third parameter of the lookupTransform function in the code represents "the time interval between the release of the location information of the two coordinate systems to be converted". We generally set it to ros::Time(0)/ros::Time(), that is, an invalid timestamp is adopted, This parameter indicates that "it is required to select the source coordinate system and target coordinate system with the shortest publishing time interval within the allowable range from the location information list of all source coordinate systems and target coordinate systems for conversion":
Therefore, we use "the source coordinate system position information published in 3s and the target coordinate system position information published in 4s" to convert the relative position relationship of the coordinate system.
2) The code structure is as follows:
3) The code design logic is as follows:
4. Preparation of launch document:
<launch> <!-- Start the first turtle node and keyboard control node --> <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" output="screen" /> <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/> <!-- Release the coordinate information of the first turtle --> <node pkg="tf2_turtle" type="getTurtle1Pose" name="getTurtle1Pose" output="screen"/> <!-- Create a second turtle and subscribe to publish coordinate information --> <node pkg="tf2_turtle" type="getTurtle2Pose" name="getTurtle2Pose" output="screen"/> <!-- coordinate transformation --> <node pkg="tf2_turtle" type="frameTransform" name="frameTransform" output="screen"/> </launch>
Generally, the logic of the whole project can be obtained from the launch file. The above launch file tells us:
1) Start the turnlesim under the turnlesim function pack_ Node:
Starting this node can generate a turnle1 node in the GUI and instantiate the publisher object of tortoise motion pose information;
2) Start turtle under the function package of turtle sim_ teleop_ Key node:
Starting the node can instantiate the subscriber object of tortoise motion control information;
3) Start custom feature pack TF2_ Getturtle 2pose node under turtle:
Starting this node can generate a turtle 2 node in the GUI and subscribe to the pose information of the turtle 2 motion node;
4) Start custom feature pack TF2_ Getturtle 1pose node under turtle:
Start this node to subscribe to the pose information of the turtle 1 movement node;
5) Start custom feature pack TF2_ frameTransform node under turtle:
Start this node to listen to the relative position relationship of the coordinate system, calculate the speed information of turnle2 from the position relationship of the coordinate system, and publish the speed information of turnle2.
5. The operation results are as follows:
