AR Tracking Chassis

Calculate transformations with new node using kown TF tree

Review transformation between 2 frames:

rosrun tf tf_echo ar_marker_153 base_footprint

Useful Script:

古月居

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
  //turtlesim提供/spawn服务
  turtlesim::Spawn srv;
  //通过call函数调用服务, 并将结果存储在srv中, 返回bool, 正常时返回true
  add_turtle.call(srv);
  //Twist含有两部分: linear[x, y, z] angular[x, y, z]
  //配置向topic:turtle2/cmd_vel发送message
  ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    //获取turtle2, turtle1 之间的 transform(从1到2的变换)
    try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),                                    transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);
    rate.sleep();
  }
  return 0;
};

Optical chassis driver node subscribes to TF and calculates spatial difference. Node drives chassis by publishing cmd_vel.

==============================================================================

My Draft Code:

#include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf/tf.h" #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/Twist.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 初始化 ros 节点 ros::init(argc,argv,"optical_driver"); // 创建 ros 句柄 ros::NodeHandle nh; // 创建 TF 订阅对象 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); // 处理订阅到的 TF // 需要创建发布 /optical/cmd_vel 的 publisher 对象 ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/optical/cmd_vel",1000); ros::Rate rate(10); while (ros::ok()) { try { //先获取 frame1 相对 frame2 的坐标信息 // geometry_msgs::TransformStamped tfs = buffer.lookupTransform("ar_marker_153","camera_link",ros::Time(0)); geometry_msgs::TransformStamped tfs = buffer.lookupTransform("marker_153","camera_link",ros::Time(0)); //根据坐标信息生成速度信息 -- geometry_msgs/Twist.h //quaternion to rpy 逆解链接 geometry_msgs::Twist twist; tf::Quaternion q( tfs.transform.rotation.x, tfs.transform.rotation.y, tfs.transform.rotation.z, tfs.transform.rotation.w); tf::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); // twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2)); // twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x); twist.linear.x = tfs.transform.translation.x; twist.linear.y = tfs.transform.translation.y; twist.linear.z = tfs.transform.translation.z; twist.angular.x = roll; twist.angular.y = pitch; twist.angular.z = yaw; //5-3.发布速度信息 -- 需要提前创建 publish 对象 pub.publish(twist); } catch(const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("错误提示:%s",e.what()); } rate.sleep(); // 6.spin ros::spinOnce(); } return 0; }

添加至CMakeList

add_executable(ar_chassis_cmd scripts/ar_chassis_cmd.cpp)

add_dependencies(ar_chassis_cmd ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(ar_chassis_cmd ${catkin_LIBRARIES} )

追加启动

<node name="ar_chassis_cmd" pkg="formal02_ar_marker_tracker" type="ar_chassis_cmd"/> =========================================================== 参考：

http://www.autolabor.com.cn/book/ROSTutorials/di-5-zhang-ji-qi-ren-dao-hang/51-tfzuo-biao-bian-huan/516-tfzuo-biao-bian-huan-shi-cao.html

=========================================================== Adding EKF might help more?