The base_control.cpp code is as follows:
/******************************************************************
The basic controller of ROS Trolley Based on serial communication has the following functions:
1. Realize ros control data through fixed format and serial communication, so as to control the movement of the car.
2. Subscribed to the / cmd_level theme, as long as you publish messages to the theme, you can achieve the movement of the control car.
3. Publish Odometer Theme/odm Serial Communication Instructions:
1. Write to serial port (1) content: left and right wheel speed, in mm/s (2) format: 10 bytes, [right wheel speed 4 bytes] [left wheel speed 4 bytes] [terminator " r n" 2 bytes]
2. Read the serial port (1) content: car x,y coordinates, direction angle, linear speed, angular speed, unit in turn: mm,mm,rad,mm/s,rad/s (2) format: 21 bytes, [X coordinate 4 bytes] [Y coordinate 4 bytes] [direction angle 4 bytes] [angular speed 4 bytes] [terminator " n" 1 byte]
*******************************************************************/
#include "ros/ros.h"// Import ROS System Contains Core Common Header Files #include <geometry_msgs/Twist.h> #Include <tf/transform_broadcaster.h>// / We need to transform "odom" reference frame to "base_link" reference frame #Include <nav_msgs/Odometry.h>// and publication of nav_msgs/Odometry message //Here are the header files needed for serial communication #include <string> #include <iostream> #include <cstdio> #include <unistd.h> #include <math.h> #include "serial/serial.h" /****************************************************************************/ using std::string; //Any identifier using the C++ standard library using std::exception; //There are also two ways to write it using std::cout; //Specify identifiers directly. For example, std::ostream rather than ostream using std::cerr; //For example, std:: cout < < std:: hex < < 3.4 < < std:: endl; using std::endl; //The most convenient way is to use namespace std; so that all identifiers defined in namespace STD are valid (exposed). As if they were declared global variables using std::vector; //#include <iostream>,#include <sstream>,#include <string>,using namespace std; /*****************************************************************************/ float ratio = 1000.0f ; //Speed conversion ratio, execution speed adjustment ratio float D = 0.2680859f ; //The distance between two wheels, in m. float linear_temp=0,angular_temp=0;//Temporary linear and angular velocities /****************************************************/ unsigned char data_terminal0=0x0d; //"/r" character carriage return unsigned char data_terminal1=0x0a; //"/n" character wrap // 0d 0a This is a self-defined data frame end flag unsigned char speed_data[10]={0}; //Data to be sent to serial port string rec_buffer; //Serial Port Data Receiving Variables //String: For a small number of string operations //StringBuilder: Suitable for large number of operations in character buffer under single thread //StringBuffer: Suitable for large number of operations in character buffer under multi-threading //Speed of left and right wheels, coordinates and direction of odometer sent to lower computer union floatData //The function of union is to realize the conversion between char array and float { float d; unsigned char data[4]; }right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular; /************************************************************/ void callback(const geometry_msgs::Twist & cmd_input)//Subscription/cmd_level topic callback function //Twist message, whose Topic is / cmd_vel, base controller subscribes to Twist message to control the motor. { string port("/dev/ttyUSB0"); //Car serial slogan unsigned long baud = 115200; //Vehicle Serial Port Baud Rate serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //Configuring Serial Port angular_temp = cmd_input.angular.z ;//Get the angular velocity of / cmd_vel, rad/s linear_temp = cmd_input.linear.x ;//Get the linear velocity of / cmd_vel. m/s //Change the speed component of the converted car to the speed of the left and right wheels left_speed_data.d = linear_temp - 0.5f*angular_temp*D ; right_speed_data.d = linear_temp + 0.5f*angular_temp*D ; //Store data to the right and left wheel speed message to be published left_speed_data.d*=ratio; //Magnification 1000 times, mm/s right_speed_data.d*=ratio;//Magnification 1000 times, mm/s for(int i=0;i<4;i++) //Save the speed of the left and right wheels in the array and send it to the serial port { speed_data[i]=right_speed_data.data[i]; speed_data[i+4]=left_speed_data.data[i]; } //Add "/r/n" after writing the speed data of left and right wheels of serial port“ speed_data[8]=data_terminal0; speed_data[9]=data_terminal1; //Write data to serial port my_serial.write(speed_data,10); } int main(int argc, char **argv)// argv is an array of pointers. Its number of elements is argc. It stores pointers to each parameter. { string port("/dev/ttyUSB0");//Car serial slogan unsigned long baud = 115200;//Vehicle Serial Port Baud Rate serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//Configuring Serial Port ros::init(argc, argv, "base_controller");//Initialization of Serial Node ros::NodeHandle n; //Define node process handles ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //Subscribe to / cmd_level topics ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //Publish a topic named odom with a message type nav_msgs. Define the message queue size to be 20, that is, after more than 20 messages, the old messages will be discarded. static tf::TransformBroadcaster odom_broadcaster;//Define a tf broadcast to publish tf transform information. geometry_msgs::TransformStamped odom_trans;//Create a tf to publish the TransformStamped type message you need to use nav_msgs::Odometry odom;//Define odometer object geometry_msgs::Quaternion odom_quat; //Quaternion variable //The covariance covariance matrix is defined to solve the uncertainty of different measurements of civil service and speed. float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x 0, 0.01, 0, 0, 0, 0, // covariance on gps_y 0, 0, 99999, 0, 0, 0, // covariance on gps_z 0, 0, 0, 99999, 0, 0, // large covariance on rot x 0, 0, 0, 0, 99999, 0, // large covariance on rot y 0, 0, 0, 0, 0, 0.01}; // large covariance on rot z //Load covariance matrix for(int i = 0; i < 36; i++) { odom.pose.covariance[i] = covariance[i];; } ros::Rate loop_rate(10); //Set periodic hibernation time and specify the frequency at which messages are published. This means 10Hz, or 10 times per second. //Rate::sleep() is used to process sleep time to control the corresponding release frequency. while(ros::ok()) //By default, roscpp implants a SIGINT processing mechanism. When Ctrl-C is pressed, ros::ok() returns false, and the loop ends. //ros::ok() returns false in several cases: //SIGINT receives (Ctrl-C) signal //When another node of the same name starts, it terminates the previous node of the same name first. //ros::shutdown() is called //All ros::NodeHandles destroyed { rec_buffer =my_serial.readline(25,"\n"); //Get data from serial port //readLine() is used to read streaming data. When reading the newline markers' n',' r'(carriage return), it follows the newline and returns the data of this line as a string. When reading all the data, it returns null. const char *receive_data=rec_buffer.data(); //Save data sent by serial port if(rec_buffer.length()==21) //Processing and publishing odometer data messages when the length of data received by serial port is correct { for(int i=0;i<4;i++)//Extraction of X, Y coordinates, direction, linear velocity and angular velocity { position_x.data[i]=receive_data[i]; position_y.data[i]=receive_data[i+4]; oriention.data[i]=receive_data[i+8]; vel_linear.data[i]=receive_data[i+12]; vel_angular.data[i]=receive_data[i+16]; } //Reduce X, Y coordinates and linear velocity by 1000 times position_x.d/=1000; //m position_y.d/=1000; //m vel_linear.d/=1000; //m/s // In order to be compatible with two-dimensional and three-dimensional functional packages and make message structures more general, the yaw angle of odometer needs to be converted to quaternion before it can be released. odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//Convert yaw angle to Quaternion //Load coordinate (tf) transformation timestamp //A timestamp is a time stamp used to indicate the uniqueness of the information. odom_trans.header.stamp = ros::Time::now(); //Father-child coordinate system for publishing coordinate transformation odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_footprint"; //tf location data: x,y,z, direction odom_trans.transform.translation.x = position_x.d; odom_trans.transform.translation.y = position_y.d; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = odom_quat; //Publish tf coordinate changes odom_broadcaster.sendTransform(odom_trans); //Load mileage timestamp odom.header.stamp = ros::Time::now(); //Father-son coordinate system of odometer odom.header.frame_id = "odom"; odom.child_frame_id = "base_footprint"; //Odometer position data: x,y,z, direction odom.pose.pose.position.x = position_x.d; odom.pose.pose.position.y = position_y.d; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; //Loading Linear Velocity and Angular Velocity odom.twist.twist.linear.x = vel_linear.d; //odom.twist.twist.linear.y = odom_vy; odom.twist.twist.angular.z = vel_angular.d; //Release odometer odom_pub.publish(odom); ros::spinOnce();//Periodic execution is necessary if there is a subscription topic in the program, otherwise the callback function will not work. loop_rate.sleep();//Cyclic dormancy } //Program periodic invocation //Ros:: spinOnce (); the // callback function must handle all problems before it can be used } return 0;
}
odom:
Odometer coordinate system, here to distinguish odom topic, these are two concepts, one is coordinate system, one is based on encoder (or vision, etc.) odometer calculation. But there is also a relationship between them. The transformed position and attitude matrix of odom topic is the tf relation of Odom -> base_link. At this point, you may wonder if Odom and map coordinates coincide. (That's the main problem I've solved by writing this blog.) I can tell you with certainty that robotic motion begins to overlap. However, with the passage of time, there is no coincidence, and the deviation is the cumulative error of odometer. How to get the tf of map -> odom? For example, gmapping will give a localization for the tf of map -> base_link, so the deviation between the estimated position and Odom position is also the coordinate system deviation between Odom and map. So, if your Odom calculation is correct, then the tf of map -> Odom is 0.
quat:
pose data acquired by oculus contains coordinate and quaternion information, but the coordinate system is different from that of CryEngine.
oculus coordinates: +x to the right, +y to the up, +z to the outside (out of the screen)
cryengine coordinates: +x to the right, +y inward (within the screen), and + z upward
Callback functions ros::spin() and ros::spinOnce()
ros::spin()
This sentence means to loop and listen for feedback functions. Loop means that when the program runs here, it will always be here. The listening feedback function means that if the node has a callback function, write ros::spin() here, and you can run the contents of the callback function when the corresponding message arrives.
For now, this sentence applies to the end of the program (because the code written after this sentence will not be executed), to the subscriber node, and there is no limit to the speed of subscription.
ros::spinOnce()
The meaning of this sentence is called back. Feedback can only be monitored, not recycled. So when you need to listen, call this function.
This function is flexible, especially when I want to control the receiving speed. Excellent effect with ros::ok()
The main loop of ROS needs to call ros::spin() or ros::spinOnce(), the difference is that the former will not return after calling, and the latter can continue to execute after calling.
In the case of ros::spin(), callbacks for all messages are generally set up at initialization, and no other background program is required to run. In this way, the user's callback function is executed every time a message arrives, which is equivalent to the program driven by message events; in the case of ros::spinOnce(), callbacks alone are not enough to complete the task, and other auxiliary programs are needed to execute such as timing tasks, data processing, and usage. User interface, etc.
Principle: In addition to the user's main program, the socket connection control process of ROS receives subscribed messages in the background. All received messages are not processed immediately, but centralized until spin() or spinOnce() executes.
I. For fast messages, attention should be paid to the reasonable control of message queue and spinOnce() time. For example, if the frequency of message arrival is 100 Hz and the execution frequency of spinOnce() is 10 Hz, then at least ensure that the reserved size in the message queue is greater than 10.
II. For users'own periodic tasks, it is best to call them side by side with spinOnce(). Even if the task is to process the data periodically, such as Kalman filtering of the received IMU data, it is not recommended to put it directly in the callback function: because of the uncertainty of communication reception, the time stability of the callback execution cannot be guaranteed.