移动小车自主导航到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="qq44642754@gmail.com">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。