ROS导航小车3 odom里程计标定（仅作个人记录）

检测机器人是否需要标定参考链接

1.打开一个新的terminal，运行rviz
   rviz窗口打开后，将fixed frame选择为odom
   关闭其他所有勾选，只保留grid和tf（如没有grid，左下角add新增）
   打开tf，下拉出来frames的内容，关闭其他所有勾选，保留baselink和odom
   rviz右侧界面可看两个重合的坐标系(baselink和odom)

 2.启动键盘控制，使用ROS键盘控制原地转360度
     对机器人（实体）的四个轮子做标记，标记此时车的位置
     键盘控制机器人原地360度转一圈（请必须记住此时旋转的方向，标定会用到），控制机器人         回到刚刚标记的位置（重合），保持机器人与标记的初始位置方向一致
     观察rviz中的2个坐标系是否重合

控制小车前进给定距离参考链接

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>
#include<time.h>

double r_dis_long;
double ix, iy, px, py;//initation & pose x,y

double dis_long;//the distance need to move
double robot_v;//the linear_velocity with the robot
//double robot_w;//the angular_velcity eith the robot

//ros::Time last_time, current_time;



void poseCallback(const nav_msgs::Odometry &p_msg){
    px = p_msg.pose.pose.position.x;
    py = p_msg.pose.pose.position.y;
   // ROS_INFO("Robot walked %.2f m",px);
    ROS_INFO("Robot walked %.2f m",py);
}

int main(int argc,char** argv){
    ros::init(argc, argv, "command_publisher");
    ros::NodeHandle nh;
    ros::Publisher command_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    // ros::Publisher  init_data_pub = nh.advertise<nav_msgs::Odometry>("/odom", 10);
    ros::Subscriber pose_sub = nh.subscribe("/odom", 10, poseCallback);
    
    std::cout << "Please Input distance(m) :"<< std::endl;
    std::cin >> dis_long;
    std::cout << "Please Input velocity(m/s) :"<< std::endl;
    std::cin >> robot_v;
    // std::cout << "Please Input radius(rad/s) :"<< std::endl;
    // std::cin >> robot_w;

    
    bool is_start = true;
    
    double count = 0;
    while(ros::ok()){
        ros::spinOnce();
        if(is_start)
        {
          //  ix = px;
              iy = py;
            is_start =false;
        }

        if(count < dis_long){
            ros::spinOnce();

            geometry_msgs::Twist com_msg;
            if( count > dis_long * 3 / 5){
                //com_msg.linear.x = robot_v / 3;
                com_msg.linear.y = robot_v / 4;
            }
            else{
               // com_msg.linear.x = robot_v;
               com_msg.linear.y = robot_v;
            }
           // ROS_INFO("Publish turtle velocity command[%.2f m/s,%.2f rad/s] distance: %.2f", com_msg.linear.x, com_msg.angular.z, count);
           ROS_INFO("Publish turtle velocity command[%.2f m/s,%.2f rad/s] distance: %.2f", com_msg.linear.y, com_msg.angular.z, count);
            command_pub.publish(com_msg);
            //count = px - ix;
             count = py - iy;
        }
        if(count >= dis_long)
        {   
           command_pub.publish(geometry_msgs::Twist());
        };
        
    }

    return 0;
}   

控制小车旋转固定角度参考链接

#include<iostream>
#include<string>
#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<geometry_msgs/Quaternion.h>
#include<nav_msgs/Odometry.h>
#include<tf/tf.h>

#define UNIT_ANGLE 180 / 3.1149

double rotation_angle, rotation_vel = 0.5;
geometry_msgs::Vector3 rpy;
tf::Quaternion q_msg;
double roll, pitch, yaw, i_yaw, m_yaw=0;

void poseCallback(const nav_msgs::Odometry &odom){
    tf::quaternionMsgToTF(odom.pose.pose.orientation, q_msg);
    tf::Matrix3x3(q_msg).getRPY(roll, pitch, yaw);
}

double T_angle(double a){
    a = fmod(a, 360);
    if(a >= 180)a = a - 360;
    else if(a <= -180)a = a + 360;
    return a;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "rotation_publisher");
    ros::NodeHandle nh;
    ros::Publisher rotation_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Subscriber pose_sub = nh.subscribe("/odom", 10, poseCallback);

    std::cout << "Please Input the rotation_angle: " << std::endl;
    std::cin >> rotation_angle ;
    // std::cout << "Please Input the rotation_vel: " << std::endl;
    // std::cin >> rotation_vel ;

    rotation_angle = T_angle(rotation_angle);

    double angle = 0;
    
    double abs_rotation_angle = abs(rotation_angle);

    bool start = true;
    double count = 0;
    while(ros::ok()){
        ros::spinOnce();
        if(start){
            i_yaw = yaw;
            start = false;
        }
        if(angle * UNIT_ANGLE < abs_rotation_angle){
            ros::spinOnce();
            geometry_msgs::Twist r_vel_msgs;
            
            if(rotation_angle < 0){
                angle = i_yaw - yaw;
                r_vel_msgs.angular.z = -rotation_vel;

                rotation_pub.publish(r_vel_msgs);
            }
            else{
                angle = yaw - i_yaw;
                r_vel_msgs.angular.z = rotation_vel;

                rotation_pub.publish(r_vel_msgs);
            }
            
            // if(yaw > m_yaw)m_yaw = yaw;
            std::cout << "yaw: %.5f" << yaw << "Angle: %.2f " << angle * UNIT_ANGLE << std::endl;
        }
        if(angle *UNIT_ANGLE >= abs_rotation_angle){

            rotation_pub.publish(geometry_msgs::Twist());
        };
        
    }

    return 0;

}

其他控制小车参考链接

https://blog.csdn.net/LL1234566/article/details/78208035?spm=1001.2101.3001.6650.5&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-5-78208035-blog-123127606.pc_relevant_3mothn_strategy_recovery&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-5-78208035-blog-123127606.pc_relevant_3mothn_strategy_recovery&utm_relevant_index=10

https://blog.csdn.net/weixin_43566648/article/details/89818426?spm=1001.2101.3001.6650.1&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-89818426-blog-107481781.pc_relevant_multi_platform_whitelistv3&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-89818426-blog-107481781.pc_relevant_multi_platform_whitelistv3&utm_relevant_index=2

https://www.theconstructsim.com/ros-qa-195-how-to-know-if-robot-has-moved-one-meter-using-odometry/

通过键盘控制小车移动和旋转固定距离或者固定角度，读取odom话题信息进行判断。

四元数到欧拉角转换参考：

#include "tf/transform_datatypes.h"
#include <nav_msgs/Odometry.h>
#include<iostream>
#include<string>
#include<ros/ros.h>
#include <math.h>



 void toEulerAngle(const double x,const double y,const double z,const double w)
{  double roll,pitch,yaw;
// roll (x-axis rotation)
    double sinr_cosp = +2.0 * (w * x + y * z);
    double cosr_cosp = +1.0 - 2.0 * (x * x + y * y);
    roll = std::atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
    double sinp = +2.0 * (w * y - z * x);
    if (std::fabs(sinp) >= 1)
        pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        pitch = std::asin(sinp);

// yaw (z-axis rotation)
    double siny_cosp = +2.0 * (w * z + x * y);
    double cosy_cosp = +1.0 - 2.0 * (y * y + z * z);
    yaw = std::atan2(siny_cosp, cosy_cosp);
//    return yaw;
   ROS_INFO("yaw %.2f ",yaw*180/3.14159);
}


int main()
{   double x,y,z,w;     
    double a[4] = {0};
	for (int i = 0; i < 4; ++i){
		std::cin >> a[i];

	}
    x=a[0];
    y=a[1];
    z=a[2];
    w=a[3];
	for (int i = 0; i < 4; ++i){
		std::cout << a[i] << std::endl;
	}
	
    toEulerAngle(x,y,z,w);

   
 return 0;


}





 /**
void odomCallback(const nav_msgs::Odometry &odom) {
 
     
      tf::Quaternion quat;
      tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
 
     
      double roll, pitch, yaw;
      tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
       
    }
 
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Subscriber quat_subscriber = n.subscribe("/odom", 1000, odomCallback);
 
    // check for incoming quaternions untill ctrl+c is pressed
    ROS_INFO("waiting for quaternion" );
    ros::spin();
    return 0;
}
**/