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 math
import rospy
import rocon_utilities.console as console
import visualization_msgs.msg as visualization_msgs
from 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