ROS python 代码转换到 C++

背景：

ROS python 代码转换到 用C++实现，我的关于ar_marker追踪的时候发现点云跳动比较明显。找到了yoc他们的，测试了一下，发现效果还可以，无奈了他们前面的几个代码都是C++的，最后测试demo用Python写的，因此有了这个。其实早在以前学习ROS by Example这本书的时候，尝试用C++写过一部分，这次算是个学习笔记吧。

yujin_ocs/yocs_ar_marker_tracking/scripts/testies.py

代码地址：https://github.com/yujinrobot/yujin_ocs/blob/devel/yocs_ar_marker_tracking/scripts/testies.py

#!/usr/bin/env python############################################################################### Imports##############################################################################import mathimport rospyimport rocon_utilities.console as consoleimport visualization_msgs.msg as visualization_msgsfrom std_msgs.msg import Float32# Can't turn it much - different error params?# Sometimes jumps x-values near the edges -> due to bad z calculations on the fringe (out by 50cm!)  # should be able to easily filter these out# a and b disappear near the edges, but I still get readings############################################################################### Functions##############################################################################class Marker:    def __init__(self, id, x, y, z):        self.id = id        self.update(x, y, z)        def update(self, x, y, z):        self.x = x        self.y = y        self.z = z        self.distance = math.sqrt(x*x + z*z)class Jagi(object):    def __init__(self):        self.baseline = 0.26        self.left_marker = Marker(id=0, x=0, y=0, z=0)        self.right_marker = Marker(id=1, x=0, y=0, z=0)        self.left_last_timestamp = rospy.get_rostime()        self.right_last_timestamp = rospy.get_rostime()        self.x_publisher = rospy.Publisher('x', Float32)        self.z_publisher = rospy.Publisher('z', Float32)        self.heading_publisher = rospy.Publisher('heading', Float32)        self.subscriber = rospy.Subscriber('/visualization_marker', visualization_msgs.Marker, self.callback)    def print_pythags_results(self):        b = self.baseline/2 + (self.left_marker.distance*self.left_marker.distance - self.right_marker.distance*self.right_marker.distance)/(2*self.baseline)        try:            a = math.sqrt(self.left_marker.distance*self.left_marker.distance - b*b)        except ValueError:            print("Value error: measured_distance=%s, b=%s" % (self.left_marker.distance, b))            return False # self.left_marker not initialised fully yet, so negative sqrt        print(console.cyan + "           a" + console.reset + " : " + console.yellow + "%s" % a + console.reset)        print(console.cyan + "           b" + console.reset + " : " + console.yellow + "%s" % b + console.reset)        return True    def print_pose_results(self):        # angle between the robot and the first marker        alpha = math.atan2(self.left_marker.x, self.left_marker.z)        alpha_degrees = alpha*180.0/math.pi        # alpha + beta is angle between the robot and the second marker         beta = math.atan2(self.right_marker.x, self.right_marker.z)        beta_degrees = beta*180.0/math.pi        # theta is the angle between the wall and the perpendicular in front of the robot        theta = math.atan2((self.left_marker.z - self.right_marker.z), (self.right_marker.x - self.left_marker.x))        theta_degrees = theta*180.0/math.pi            print(console.cyan + "       alpha" + console.reset + " : " + console.yellow + "%s" % alpha_degrees + " degrees" + console.reset)        print(console.cyan + "        beta" + console.reset + " : " + console.yellow + "%s" % beta_degrees + " degrees" + console.reset)        print(console.cyan + "       theta" + console.reset + " : " + console.yellow + "%s" % theta_degrees + " degrees" + console.reset)                # M1 = (self.left_marker.x, self.left_marker.z)        # M2 = (self.right_marker.x, self.right_marker.z)        # M3 = M1 + (M2 - M1)/2   # midpoint of M1, M2        # Target stop position relative to marker mid point (40cm perpendicularly out)        # M4 = (-0.4*sin(theta), -0.4*cos(theta))        # Target stop position        # M5 = M3 + M4        target_x = self.left_marker.x + (self.right_marker.x - self.left_marker.x)/2 - 0.4*math.sin(theta)        target_z = self.left_marker.z + (self.right_marker.z - self.left_marker.z)/2 - 0.4*math.cos(theta)        target_heading = math.atan2(target_x, target_z)        target_heading_degrees = target_heading*180.0/math.pi                print(console.cyan + "      target" + console.reset + " : " + console.yellow + "(x=%s, z=%s, heading=%s)" % (target_x, target_z, target_heading_degrees) + console.reset)        self.x_publisher.publish(Float32(target_x))        self.z_publisher.publish(Float32(target_z))        self.heading_publisher.publish(Float32(target_heading_degrees))        def callback(self, data):        if data.id == 0:            self.right_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z)            self.right_last_timestamp = data.header.stamp            print(console.cyan + "r: [x, y, z]" + console.reset + " : " + console.yellow + "[%s, %s, %s]" % (self.right_marker.x, self.right_marker.y, self.right_marker.z) + console.reset)            print(console.cyan + "r: d" + console.reset + "         : " + console.yellow + "%s" % self.right_marker.distance + console.reset)        else:            self.left_last_timestamp = data.header.stamp            self.left_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z)            print(console.cyan + "l: [x, y, z]" + console.reset + " : " + console.yellow + "[%s, %s, %s]" % (self.left_marker.x, self.left_marker.y, self.left_marker.z) + console.reset)            print(console.cyan + "l: d" + console.reset + "         : " + console.yellow + "%s" % self.left_marker.distance + console.reset)        #if math.fabs(self.left_last_timestamp - self.right_last_timestamp) < 0.1:        if self.left_last_timestamp == self.right_last_timestamp:            if self.print_pythags_results():                self.print_pose_results()############################################################################### Main##############################################################################if __name__ == '__main__':    #visualization_msgs/Marker    rospy.init_node('roles_and_apps')    jagi = Jagi()    rospy.spin()

C++ 的改写如下

#include <ros/ros.h>#include <visualization_msgs/Marker.h>#include <std_msgs/Float64.h>#include <math.h>class Marker{public:    int id;    float x;    float y;    float z;    float distance;public:    Marker()    {    }    Marker(int id, double x, double y, double z)    {        this->id = id;        this->update(x, y, z);    }    void update( double x, double y, double z)    {        this->x = x;        this->y = y;        this->z = z;       this->distance = sqrt(x*x + z*z);    }};class Jagi : public Marker{private:    double baseline;    Marker left_marker;    Marker right_marker;//    Marker left_marker(0,0.0,0.0,0.0);//    Marker right_marker(0, 0.0, 0.0, 0.0);    ros::Time left_last_timestamp, right_last_timestamp;    ros::Publisher x_publisher, z_publisher, heading_publisher;    ros::Subscriber subscriber;    ros::NodeHandle n_;public:    Jagi(const ros::NodeHandle& node_handle) : n_(node_handle)    {        this->baseline = 0.26; // Kinect baseline?        this->left_marker = Marker(0, 0.0, 0.0, 0.0) ;// id        this->right_marker = Marker(1, 0.0, 0.0, 0.0);        this->left_last_timestamp = ros::Time::now();        this->right_last_timestamp = ros::Time::now();        this->x_publisher = n_.advertise<std_msgs::Float64>("x", 10);        this->z_publisher = n_.advertise<std_msgs::Float64>("z", 10);        this->heading_publisher = n_.advertise<std_msgs::Float64>("heading", 10);        this->subscriber = n_.subscribe("/visualization_marker", 20, &Jagi::callback, this);    }    void callback( const visualization_msgs::Marker& data)    {        if(data.id == 0)        {            this->right_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z);            this->right_last_timestamp = data.header.stamp;            std::cout<< "r: [x, y, z] : " <<this->right_marker.x<<" "<< this->right_marker.y<<" "<< this->right_marker.z <<std::endl;            std::cout<<"r: d: "<<this->right_marker.distance<<std::endl;        }else        {            this->left_last_timestamp = data.header.stamp;            this->left_marker.update(data.pose.position.x, data.pose.position.y, data.pose.position.z);            std::cout<< "l: [x, y, z] : " << this->left_marker.x<<" "<< this->left_marker.y<<" "<< this->left_marker.z <<std::endl;            std::cout<<"l: d: "<< this->left_marker.distance<<std::endl;        }        if (this->left_last_timestamp == this->right_last_timestamp)        {            if (this->print_pythags_results())            {                this->print_pose_results();            }        }    }    bool print_pythags_results()    {        double a;        double b = this->baseline/2 + (this->left_marker.distance*this->left_marker.distance - this->right_marker.distance*this->right_marker.distance)/(2*this->baseline);        try {             a =  sqrt(this->left_marker.distance*this->left_marker.distance - b*b);        } catch (std::exception &e)        {            printf("Value error: measured_distance=%lf, b=%lf\n" ,this->left_marker.distance, b);            return false;// this->left_marker not initialised fully yet, so negative sqrt        }        printf(" a : %lf\n" , a);        printf(" b : %lf\n" , b);        return true;    }    void print_pose_results()    {        std_msgs::Float64 send_data;        // angle between the robot and the first marker        double alpha = atan2(this->left_marker.x, this->left_marker.z);        double alpha_degrees = alpha*180.0/M_PI;        // alpha + beta is angle between the robot and the second marker        double beta = atan2(this->right_marker.x, this->right_marker.z);        double beta_degrees = beta*180.0/M_PI;        // theta is the angle between the wall and the perpendicular in front of the robot        double theta = atan2((this->left_marker.z - this->right_marker.z), (this->right_marker.x - this->left_marker.x));        double theta_degrees = theta*180.0/M_PI;        printf( " alpha : %lf degrees" , alpha_degrees);        printf( " beta : %lf degrees" , beta_degrees );        printf( " theta : %lf degrees" , theta_degrees);        // M1 = (this->left_marker.x, this->left_marker.z)        // M2 = (this->right_marker.x, this->right_marker.z)        // M3 = M1 + (M2 - M1)/2   // midpoint of M1, M2        // Target stop position relative to marker mid point (40cm perpendicularly out)        // M4 = (-0.4*sin(theta), -0.4*cos(theta))        // Target stop position        // M5 = M3 + M4       double target_x = this->left_marker.x + (this->right_marker.x - this->left_marker.x)/2 - 0.4* sin(theta);       double target_z = this->left_marker.z + (this->right_marker.z - this->left_marker.z)/2 - 0.4* cos(theta);       double target_heading = atan2(target_x, target_z);       double target_heading_degrees = target_heading*180.0/M_PI;        printf( " target : (x=%lf, z=%lf, heading=%lf)" , target_x, target_z, target_heading_degrees);        send_data.data = target_x;        this->x_publisher.publish(send_data);        send_data.data = target_z;        this->z_publisher.publish(send_data);        send_data.data = target_heading_degrees;        this->heading_publisher.publish(send_data);    }};int main(int argc, char **argv){    ros::init(argc, argv, "ar_auto_docking");    ros::NodeHandle n("~");    Jagi Jagi(n);    ros::spin();    return 0;}

其中，主要是有一个类的继承和pub发布时候的强制转化，那里花费了较多时间debug。

$ roslaunch yocs_ar_marker_tracking testies-3dsensor.launch

$ roslaunch yocs_ar_marker_tracking tracking.launch

$ rosrun yocs_ar_marker_tracking test_node