在本章我们介绍如何通过卓尔-ARM抓取苹果，并放置。
先来看看整体流程：
卓尔-ARM抓取苹果流程
我们使用颜色识别，识别苹果的颜色，然后通过机械臂抓取苹果，最后放置苹果。
首先，确保您已经使用卓尔-ARM完成了建图与导航，和机械臂相关的操作。
然后，确保机械臂已经上电，可以在WEBAPP端进行控制。
将苹果放在机器人右侧某个位置，确保摄像头可以看到。
接下来，我们需要在机载端运行脚本，进行颜色识别，抓取苹果。
cd ~/robot_shell
./11-apple_carry.sh
然后打开新的终端输入rviz，在rviz中，选择OpenConfig
在弹框中，选择下面的文件：
即可加载导航相关配置：
在上方2D Nav Goal按钮选择相应的按键即可实现自主导航。
打开新的终端输入rqt，如下方式，配置出苹果的HSV数值：
这样，机械臂在掠过苹果上方时，就能够识别并定位苹果了，为后续抓取做准备。
然后打开终端输入：
rosrun grab_demo nav_apple
即可实现抓取后导航放置。
我们来看看nav_apple的代码：
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
#include <geometry_msgs/Twist.h>
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;
ros::NodeHandle nh;
ros::Publisher vel_pub;
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100);
//待发送的 目标点 在 map 坐标系下的坐标位置
goal1.target_pose.pose.position.x = 1.06;
goal1.target_pose.pose.position.y = -0.32;
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 = 0.0;
goal2.target_pose.pose.position.y = 0.0;
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();
system("rosrun grab_demo apple_grab color_pose_link");
ros::Rate loop_rate(10);
for(int i=0; i<80; i++)
{
geometry_msgs::Twist vel;
vel.angular.z = 0;
vel.linear.x = 0.3;
vel_pub.publish(vel);
loop_rate.sleep();
ROS_INFO("111111");
}
ac.sendGoal(goal1);
ROS_INFO("Send Goal 1 !!!");
ac.waitForResult();
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)//如果到达目标点，就执行括号中的任务
{
system("rosrun grab_demo arm_put");
}
else
{
ROS_WARN("The Goal Planning Failed for some reason");
}
return 0;
}
在上述代码中，我们首先运行抓取这个节点，然后运行导航节点，到达目标点后，进行运行放置节点。
运行节点是通过C++的system函数实现，system函数可以调用系统命令，这样我们就可以在C++代码中调用shell脚本了。
接下来我们来看看app_grab的源码：
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/Twist.h"
#include <string>
#include <stdlib.h>
#include <grab_demo/position.h>
#include <std_msgs/String.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <arm_demo/lebai_demo.h>
#include <lebai_msgs/SetGripper.h>
lebai_msgs::SetGripper p_srv;
lebai_msgs::SetGripper f_srv;
void set_gripper_position(int angle,ros::NodeHandle &nh)
{
ros::ServiceClient p_client = nh.serviceClient<lebai_msgs::SetGripper>("/io_service/set_gripper_position");
p_srv.request.val=angle;
p_client.call(p_srv);
}
void set_gripper_force(int force,ros::NodeHandle &nh)
{
ros::ServiceClient f_client = nh.serviceClient<lebai_msgs::SetGripper>("/io_service/set_gripper_force");
f_srv.request.val=force;
f_client.call(p_srv);
}
int main(int argc, char *argv[])
{
int grip_position;
double MaxVelocity=0.1;//Velocity:0.1-1
std::string target_pose_frame=argv[1];//传入到达目标点的TF坐标
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"start_grab_node");
// 3.创建 ros 句柄
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;
set_gripper_force(5,nh);
LeBai lebai("manipulator","lebai_gripper");
lebai.set_MaxVelocity_Accelerate(1,0.5);//设置运动速度，如需更快的速度可在lebai_lm3_support/urdf/s300_lm3.xacro 文件修改各个jointx_vel_limit的值
lebai.move_Look_Higher();
sleep(1);
lebai.move_Look_Right_higher();
geometry_msgs::Pose target_pose;
set_gripper_position(100,nh);
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ROS_INFO("tf coordinate transforming....");
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("base_link",target_pose_frame,ros::Time(0),ros::Duration(100));//监听目标TF坐标系相对base_link的位姿
target_pose.position.x=tfs.transform.translation.x;
target_pose.position.y=tfs.transform.translation.y;
target_pose.position.z=tfs.transform.translation.z;
lebai.moveP(target_pose.position.x,target_pose.position.y,target_pose.position.z-0.05);
set_gripper_position(0,nh);
sleep(1);
lebai.move_Look_Right_higher();
sleep(1);
lebai.move_Look_Higher();
ros::shutdown();
spinner.stop();
return 0;
}
在上述代码中，我们首先设置了机械臂的速度，然后通过TF坐标系的转换，获取到苹果的坐标，然后通过机械臂的moveP函数，移动到苹果的上方，然后通过设置夹爪的角度，抓取苹果。
颜色识别夹取与二维码标签夹取步骤一样，仅在识别目标位姿的方法不同。首先需要订阅相机发布的图像话题获取目标物体的HSV颜色阈值，对此阈值范围内的目标物体进行图像处理，获取目标中心点的像素值，再通过相机内参矩阵计算出 XY 坐标，Z 值数据可通过深度相机获取，然后以话题的形式发布目标物体的位姿。