角度校准代码

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include <cmath>
#include <string>
#include <iostream>
#include "tf/tf.h"
#include "tf/transform_listener.h"
#include "dynamic_reconfigure/server.h"
#include "xtark_linear_calibrate/xtark_angular_calibrateConfig.h"
#include "signal.h"
#include "stdio.h"

double angular_;
double odom_angular_scale_correction_;//modify param
double test_vel_;
double tolerance_;
double aodom_angular_start_;

bool start_test_ = false;

geometry_msgs::Twist cmd_vel_;

std::string cmd_vel_topic_ = "cmd_vel";
std::string odom_link_;
std::string base_link_;
ros::Publisher angular_pub_;

void shutdowm(int signum)
{
    cmd_vel_.linear.x = 0;
    angular_pub_.publish(cmd_vel_);
    ROS_INFO(" Ctrl_C Break!!!");
    //ros::shutdown();
    //exit(signum);
    ros::shutdown();
    //exit(0);

}

void dynamic_callback(xtark_linear_calibrate::xtark_angular_calibrateConfig &config)
{
    std::cout<<"reconfigure callback"<<std::endl;
   
   
    base_link_ = config.base_link;
    odom_link_ = config.odom_link;
    start_test_ = config.start_test?true:false;
    cmd_vel_topic_ = config.cmd_vel;
    odom_angular_scale_correction_ = config.odom_angular_scale_correction;
    tolerance_ = config.tolerance;
    angular_ =  config.angle;
    test_vel_ = config.test_vel;
   // std::cout<<"config.start_test"<<start_test_<<std::cout;
   // ROS_INFO("ENter dynamic server");
   
}

double normalize_angle(double angle)
{
    
    while (angle > 180 ||angle <-180)
    {
        if(angle > 180)angle -= 360;
        else if(angle < -180)angle = angle + 360;

    } 
    return angle;

}

double start_angular;
 double laset_angle;
    double now_angle;
    double delta_angle;
    double turn_angle;
int main(int argc, char** argv)
{

   

    
    ros::init(argc, argv, "angluar_calibrate");

    ros::NodeHandle node_;

    tf::StampedTransform my_tf;
    tf::TransformListener m_listener_;

    angular_pub_ = node_.advertise<geometry_msgs::Twist>(cmd_vel_topic_, 10);

    signal(SIGINT, shutdowm);

    dynamic_reconfigure::Server<xtark_linear_calibrate::xtark_angular_calibrateConfig> server;
    dynamic_reconfigure::Server<xtark_linear_calibrate::xtark_angular_calibrateConfig>::CallbackType callback;

    callback = boost::bind(&dynamic_callback, _1);
    server.setCallback(callback);
   
    try{
        m_listener_.waitForTransform(odom_link_, base_link_, ros::Time(0), ros::Duration(1.0));
        m_listener_.lookupTransform(odom_link_, base_link_, ros::Time(0), my_tf);

    }catch(tf::TransformException ex)
    {
        
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }

   start_angular = getYaw(my_tf.getRotation());
    //double start_angular = my_tf.getRotation().getYaw()) * (180.0 / 3.14);
    start_angular = start_angular * 180 / M_PI;
    laset_angle = normalize_angle(start_angular);

    start_test_ = 0;
    ros::Rate loop(10);
    while(ros::ok())
    {
        //std::cout<<"start_test : "<<start_test_<<std::endl;
        if(start_test_ == 1)
        {
            m_listener_.lookupTransform(odom_link_, base_link_, ros::Time(0), my_tf);
            //now_angle = my_tf.getRotation().getAngle)) ;
             now_angle = getYaw(my_tf.getRotation()) * 180 / M_PI;
            //std::cout<<"now_angle: "<<now_angle<<std::endl;
            //now_angle = normalize_angle(now_angle);
            
            delta_angle = normalize_angle(now_angle - laset_angle);

            delta_angle = delta_angle * odom_angular_scale_correction_;
            turn_angle += delta_angle;
            auto error = angular_ - turn_angle;
             //std::cout<<"turn_angle: "<<turn_angle<<std::endl;
            std::cout<<"error: "<<error<<std::endl;

            laset_angle = now_angle;

            if(abs(error) > tolerance_)
            {
                test_vel_ = error>0? test_vel_:-test_vel_;
                //else test_vel_ = error>0? -test_vel_: test_vel_;

                cmd_vel_.angular.z = test_vel_;
                angular_pub_.publish(cmd_vel_);

            }
            else{
                
                cmd_vel_.angular.z = 0;
                angular_pub_.publish(cmd_vel_);

            }

        }
        else
        {
            turn_angle = 0;
            cmd_vel_.angular.z = 0;
            angular_pub_.publish(cmd_vel_);

        }

        ros::spinOnce();
        loop.sleep();

    }

}

cfg:

#!/usr/bin/python3
# _*_ coding:utf-8 _*_

PACKAGE = "xtark_linear_calibrate"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("start_test",   bool_t,                   0, "An Integer parameter",  False)
gen.add("odom_link",    str_t,                    0, "An Integer parameter",  "odom")
gen.add("base_link",    str_t,                    0, "A double parameter",    "base_footprint")
gen.add("cmd_vel",      str_t,                    0, "A string parameter",    "cmd_vel")
gen.add("tolerance",    double_t,                 0, "A Boolean parameter",    0.5, 0, 1)
gen.add("angle",        double_t,                 0, "An Integer parameter",   360.0,  0.0, 360.0)
gen.add("odom_angular_scale_correction", double_t, 0, "A double parameter",     1.0, -2, 2)
gen.add("test_vel",     double_t,                 0, "A string parameter",     0.1 ,-1 ,1)

log_enum = gen.enum([gen.const("info", int_t, 0, "log print flag:INFO"),
                      gen.const("warn", int_t, 1, "log print flag:WARN"),
                      gen.const("error", int_t, 2, "log print flag:ERROR")],
                      "Set Log Level"
)

gen.add("log_level", int_t, 0, "Set Log Level", 0 , 0, 2, edit_method=log_enum)

exit(gen.generate(PACKAGE, "xtark_angular_calibrate", "xtark_angular_calibrate"))