背景:
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