ROS算法融合

参考网址：

<u>http://docs.ros.org/api/nav_core/html/classnav__core_1_1BaseLocalPlanner.html</u>

<u>https://www.cnblogs.com/sakabatou/p/8297479.html</u>

<u>https://www.ncnynl.com/archives/201708/1887.html</u>

1.下载ＤＷＡ源码包

<u>https://github.com/ros-planning/navigation/tree/kinetic-devel</u>

２．把dwa_local_planner文件夹放到自己工作空间

３．主要接口

void initialize(std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmap_ros)**---初始化

bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)---设置全局路径

bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)---计算控制速度

bool isGoalReached()---判断是否达到目标点

４．在setPlan获取路径点 orig_global_plan

5. 在 computeVelocityCommands****加入自己的算法

6. move_base****会在调用 computeVelocityCommands****之前调用isGoalReached()判断是否到达目标

7. computeVelocityCommands****和isGoalReached()****可通过监听map到base_link的tf变换获取小车位置

８．

tf::Stamped<tf::Pose> robot_vel;

odom_helper_.getRobotVel(robot_vel);

tf::TransformListener listener;

tf::StampedTransform transform;

try{

listener.waitForTransform("/map","/base_link", ros::Time(0),ros::Duration(3.0));

listener.lookupTransform("/map","/base_link", ros::Time(0),transform);

}catch(tf::TransformException &ex){

ROS_ERROR("%s",ex.what());

}

//odom_callback()

current.x_pos = transform.getOrigin().x();

current.y= transform.getOrigin().y();

current.theta = tf::getYaw(transform.getRotation());

current.vel = robot_vel.getOrigin().getX();