移动小车自主导航到ArMarker标签前面

控制移动机器人自主导航并停到标签(AR Marker)之前（move_test.cpp ）。 这个demo一共分为三个部分，第一部分为订阅二维码的topic，读取出二维码的位姿。第二部分为将所得的二维码位置进行tf变化，得到二维码在/map下的位置。第三部分为让小车自主导航到标签前面。

move_test.cpp

#include "move_test.h"

MoveTest::MoveTest() : get_pose(0), move_finish(0), robot_move(0), getflagsuccess(0), move_base("move_base", true)

{
    
    
  ros::AsyncSpinner spinner(1);
  spinner.start();
  pose_sub = nh_.subscribe("/visualization_marker", 10, &MoveTest::poseCallback, this);
  pose = nh_.serviceClient<airface_drive_msgs::getpose>("/airface_driver/get_tf_pose");
}

MoveTest::~MoveTest()
{
    
    
  cout << "delete the class" << endl;
}

void MoveTest::poseCallback(const visualization_msgs::Marker &marker_tmp)
{
    
    
  if (robot_move == 0)
  {
    
    
    ROS_INFO("reading the pose of Marker");
    Marker_pose_tmp.header = marker_tmp.header;
    cout << Marker_pose_tmp.header << endl;
    Marker_pose_tmp.pose = marker_tmp.pose;
    Marker_pose_tmp.pose.position.z -= 0.8; //0.8m in front of the Marker
    if (Marker_pose_tmp.pose.position.x != 0)
    {
    
    
      get_pose = 1;
    }
  }
}

void MoveTest::transform_tf()
{
    
    

  if (get_pose == 1)
    ROS_INFO("get the pose of Marker, tansform now"); // Marker pose in camera link to pose in map
  {
    
    
    try
    {
    
    
      listener.transformPose("/map", Marker_pose_tmp, Marker_pose);
      getflagsuccess = 1;
      robot_move = 1;
      ROS_INFO("Transform successed");
    }
    catch (tf::TransformException &ex)
    {
    
    
      ros::Duration(0.5).sleep();
      getflagsuccess = 0;
      std::cout << "Transform failed, other try" << std::endl;
    }
    if (getflagsuccess)
    {
    
    
      airface_drive_msgs::getpose g;
      pose.call(g);
      Marker_pose.pose.orientation = tf::createQuaternionMsgFromYaw(g.response.yaw);
      cout << "Target pose was :\n"
           << Marker_pose.pose.position << endl;
      Marker_pose.pose.position.z = 0;
    }
  }
}

void MoveTest::goSP()
{
    
    

  //connet to the Server, 5s limit
  while (!move_base.waitForServer(ros::Duration(5.0)))
  {
    
    
    ROS_INFO("Waiting for move_base action server...");
  }

  ROS_INFO("Connected to move base server");

  //set the targetpose
  move_base_msgs::MoveBaseGoal goal;
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
  goal.target_pose.pose.position.x = Marker_pose.pose.position.x;
  goal.target_pose.pose.position.y = Marker_pose.pose.position.y;

  goal.target_pose.pose.orientation.z = Marker_pose.pose.orientation.z;
  goal.target_pose.pose.orientation.w = Marker_pose.pose.orientation.w;

  ROS_INFO("Sending goal");
  move_base.sendGoal(goal);

  move_base.waitForResult();

  if (move_base.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Goal succeeded!");
  else
    ROS_INFO("Goal failed");
}

void MoveTest::initMove()
{
    
    

  ros::AsyncSpinner spinner(1);
  spinner.start();
  transform_tf();
  if (getflagsuccess == 1)
  {
    
    
    ROS_INFO("get the Targetpose. now go to the Targetpose");
    goSP();
    sleep(2);
    move_finish = 1;
  }
}

int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "move_test");
  MoveTest movetest;

  while (ros::ok())
  {
    
    
    ros::spinOnce();
    movetest.initMove();
    if (movetest.move_finish == 1)
    {
    
    
      ros::shutdown();
    }
  }
  return 0;
}

move_test.h

#ifndef _MOVE_TEST_H_
#define _MOVE_TEST_H_

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include "visualization_msgs/Marker.h"
#include <geometry_msgs/PoseStamped.h>
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <airface_drive_msgs/getpose.h>

using namespace std;

class MoveTest
{
    
    
private:
    move_base_msgs::MoveBaseGoal goal;
    ros::NodeHandle nh_;
    ros::Subscriber pose_sub;
    tf::TransformListener listener;
    geometry_msgs::PoseStamped Marker_pose, Marker_pose_tmp;
    ros::ServiceClient pose;
    int get_pose, robot_move, getflagsuccess ;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base;
public:
    MoveTest();
    ~MoveTest();
    int move_finish;
    void transform_tf();
    void goSP(); // go to the startpoint
    void initMove();
    void poseCallback(const visualization_msgs::Marker &marker_tmp);
};

#endif

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(learn_action)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

find_package(OpenCV REQUIRED) 
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib
  actionlib_msgs
  airface_drive_msgs
  tf
  rostime
  sensor_msgs
  message_filters
  cv_bridge
  image_transport
  compressed_image_transport
  compressed_depth_image_transport
  geometry_msgs
)


find_package(Boost REQUIRED)

add_action_files(
  FILES
  TurtleMove.action
)

generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs
)
 
catkin_package(
 CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
 
include_directories(
 include
 ${
    
    catkin_INCLUDE_DIRS}
 ${
    
    boost_INCLUDE_DIRS}
 ${
    
    OpenCV_INCLUDE_DIRS}
# ${
    
    TinyXML_INCLUDE_DIRS}
 "/usr/include/eigen3"
)

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${
    
    catkin_LIBRARIES} ${
    
    boost_LIBRARIES})
add_dependencies(TurtleMove_client ${
    
    PROJECT_NAME}_gencpp)
 
add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${
    
    catkin_LIBRARIES} ${
    
    boost_LIBRARIES})
add_dependencies(TurtleMove_server ${
    
    PROJECT_NAME}_gencpp)

add_executable(move_test src/move_test.cpp)
target_link_libraries(move_test ${
    
    catkin_LIBRARIES} ${
    
    boost_LIBRARIES})
add_dependencies(move_test ${
    
    PROJECT_NAME}_gencpp)

add_executable(move_object src/move_object.cpp)
target_link_libraries(move_object
  ${
    
    catkin_LIBRARIES}
  ${
    
    OpenCV_LIBRARIES}
)

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>learn_action</name>
  <version>0.0.1</version>
  <description>The learn_action package</description>
  <maintainer email="[email protected]">patience</maintainer>
  <license>TODO</license>
 
 <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
 
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
 
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend> 
 
 
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
 
  </export>
</package>

注意：在头文件中用了我自定义的头函数 #include <airface_drive_msgs/getpose.h>来获取当前位置状态。大家可以根据自己的代码进行更改。例如说定义运动到标签是的角度yaw=0。