卓尔-ARM移动视觉抓取与视觉抓取类似，不过是要与导航定位结合起来。导航到点后再通过视觉抓取二维码，然后导航到另一个点，完成放置。
这里我们直接看源码：
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
using namespace std;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char **argv)
{
ros::init(argc, argv, "send_goals_node");
MoveBaseClient ac("move_base", true);
ac.waitForServer();
move_base_msgs::MoveBaseGoal goal1;
move_base_msgs::MoveBaseGoal goal2;
move_base_msgs::MoveBaseGoal goal3;
//待发送的 目标点 在 map 坐标系下的坐标位置
goal1.target_pose.pose.position.x = 1.798;
goal1.target_pose.pose.position.y = -0.012;
goal1.target_pose.pose.orientation.z = 0.006;
goal1.target_pose.pose.orientation.w = 1.0;
goal1.target_pose.header.frame_id = "map";
goal1.target_pose.header.stamp = ros::Time::now();
goal2.target_pose.pose.position.x = 2.398;
goal2.target_pose.pose.position.y = -0.012;
goal2.target_pose.pose.orientation.z = 0.006;
goal2.target_pose.pose.orientation.w = 1.0;
goal2.target_pose.header.frame_id = "map";
goal2.target_pose.header.stamp = ros::Time::now();
goal3.target_pose.pose.position.x = 0;
goal3.target_pose.pose.position.y = 0;
goal3.target_pose.pose.orientation.z = 0;
goal3.target_pose.pose.orientation.w = 1;
goal3.target_pose.header.frame_id = "map";
goal3.target_pose.header.stamp = ros::Time::now();
ac.sendGoal(goal1);
ROS_INFO("Send Goal 1 !!!");
ac.waitForResult();
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)//如果到达目标点，就执行括号中的任务
{
system("rosrun grab_demo start_grab ar_pose_link ");
}
else
{
ROS_WARN("The Goal Planning Failed for some reason");
}
ac.sendGoal(goal2);
ROS_INFO("Send Goal 2 !!!");
ac.waitForResult();
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
system("rosrun grab_demo arm_put");
}
else
{
ROS_WARN("The Goal Planning Failed for some reason");
}
ac.sendGoal(goal3);
ROS_INFO("Send Goal 3 !!!");
ac.waitForResult();
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("OJBK!!!!!");
}
else
{
ROS_WARN("The Goal Planning Failed for some reason");
}
return 0;
}
这里我们通过导航到点后，执行视觉抓取，然后导航到另一个点，执行放置。这里我们需要在goal1和goal2中填入导航的目标点，然后在system中填入执行的命令。
注意，这里的goal1和goal2是ROS坐标系下，相对于建图起点的位置。