一起做ROS-DEMO系列 (3):写一个简易的基于标签识别的云台、手臂跟踪

• 最近做了一个在MOVO机器人上实现的demo，即基于标签识别让头部与机械臂去自动跟踪到标签对应的位置，发现在摄像头视角看到标签不断跟踪标签使其保持在正中央还是挺有意思的。这是做出来的效果：

1、本文用到的相关内容

• ar_track_alaver标签识别
• tf坐标系变换
• moveit API接口

具体学习可参考前两篇中的介绍。

2、整体逻辑

这个跟踪的逻辑比较简单，大致如下：

3、cpp代码

/* follow.cpp
author : Wangxianwei Wenligang
 2019.7.17
 */
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Vector3.h>
#include <visualization_msgs/Marker.h>
#include <math.h>
#include <tf/transform_listener.h>

/* pan运动角度 */
float pan_angle = 0, pan_angleOld = 0;                                                          /* 单位rad */
/* tilt运动角度 */
float tilt_angle = 0, tilt_angleOld = 0;
/* 头部运动范围 */
float	tiltAngle_u	= 0.09, tiltAngle_d = -0.09, panAngle_u = 0.09, panAngle_d = -0.09;     
float	pan_angleMax	= 1.57, tilt_angleMax = 1.57;
float	pan_angleMin	= -1.57, tilt_angleMin = -1.57;

int main( int argc, char** argv )
{
	static const std::string PLANNING_GROUP = "head";
	ros::init( argc, argv, "follow" );
	bool		success;
	ros::NodeHandle node;
	tf::TransformListener	listener; /* 定义tf监听器 */
	ros::AsyncSpinner	spinner( 3 );
	spinner.start();

	/* 实例化moveit接口类 */
	moveit::planning_interface::MoveGroup::Plan	my_plan;
	moveit::planning_interface::MoveGroup		group( "head" );

	moveit::planning_interface::MoveGroup::Plan	left_arm_plan;
	moveit::planning_interface::MoveGroup		left_arm_group( "left_arm" );
/* 定义存放变换关系的变量 */
	tf::StampedTransform transform;
	while ( ros::ok() )
	{
		/* 在角度小于多少度之后不用转动，直接对坐标点进行控制抓取，如果在范围之外控制头部转动后进行计算，最后进行手臂控制 */

/**************云台运动控制部分**************/
		try
		{
			listener.lookupTransform( "pan_base_link", "ar_marker_5", ros::Time( 0 ), transform );
		}
		catch ( tf::TransformException &ex )
		{
			ROS_ERROR( "%s", ex.what() );
			ros::Duration( 0.3 ).sleep();
			continue;
		}
		float	tra_x	= transform.getOrigin().x();
		float	tra_y	= transform.getOrigin().y();
		float	tra_z	= transform.getOrigin().z();
		std::cout << "tra-x=" << tra_x << " tra-y=" << tra_y << "tra-z" << tra_z << std::endl;

		pan_angle	= atan( -tra_y / tra_x );
		tilt_angle	= atan( (tra_z - 0.25) / tra_x );

		if ( (pan_angle <= panAngle_u && pan_angle >= panAngle_d) && (tilt_angle <= tiltAngle_u && tilt_angle >= tiltAngle_d) )
		{
			/* 在范围内跳过执行 */
		}else  { /* 在范围外控制云台进行转动 */
			std::cout << "pan_angle:" << pan_angle << std::endl;
			std::cout << "tilt_angle:" << tilt_angle << std::endl;

			std::vector<double> joint_group_positions;

			joint_group_positions.push_back( pan_angle );                                                                                                                                                   /* 关节的增量值 */
			joint_group_positions.push_back( tilt_angle );
			/* 判断超限 */
			if ( (joint_group_positions[0] < pan_angleMin) || (joint_group_positions[0] > pan_angleMax) || (joint_group_positions[0] < tilt_angleMin) || (joint_group_positions[0] > tilt_angleMax) )       /* 超限 */
			{
				joint_group_positions[0]	= 0;                                                                                                                                                    /* 回零点 */
				joint_group_positions[1]	= 0;
			}

			group.setJointValueTarget( joint_group_positions );
			success = group.plan( my_plan );
			ROS_INFO( "head move! %s", success ? "" : "FAILED" );
			if ( success == moveit_msgs::MoveItErrorCodes::SUCCESS )
				group.move();
		} 
		ros::Duration( 0.5 ).sleep();
		/**************云台运动控制部分结束**************/


		/**************手臂运动控制部分**************/

		try
		{
			listener.lookupTransform( "base_link", "ar_marker_5", ros::Time( 0 ), transform );
		}
		catch ( tf::TransformException &ex )
		{
			ROS_ERROR( "%s", ex.what() );
			ros::Duration( 0.3 ).sleep();
			/* continue; */
		}

		geometry_msgs::Pose target_pose1;
		target_pose1.orientation.w	= 1.0;
		target_pose1.position.x		= transform.getOrigin().x() - 0.15; /* 控制机械臂末端与目标标签在x方向上距离0.15m */
		target_pose1.position.y		= transform.getOrigin().y();
		target_pose1.position.z		= transform.getOrigin().z();
		std::cout << "tra-x=" << target_pose1.position.x << " tra-y=" << target_pose1.position.y << "tra-z" << target_pose1.position.z << std::endl;


		left_arm_group.setPoseTarget( target_pose1 );
		success = left_arm_group.plan( left_arm_plan );

		ROS_INFO( "arm_left(pose goal) %s", success ? "" : "FAILED" );
		if ( success == moveit_msgs::MoveItErrorCodes::SUCCESS )
			left_arm_group.move();
	/**************手臂运动控制部分结束**************/

	}//while (ros::ok()) finished

	return(0);
};

4、本代码的一些缺陷和改进点

• 因为是调用了moveit接口，所以通过了moveit中的轨迹规划、碰撞检测后控制云台及机械臂移动，导致机械臂与云台没有那种即刻反应的效果。想要做出那种实时的跟踪，可以直接给云台发送位置指令，想要更平滑的控制的话，可以借助云台的速度控制模式，写一个简单的PID控制。机械臂想要做到实时跟踪就比较难了，直接控制关节转动容易导致一些碰撞问题，只能尽量采用速度更快的轨迹求解方法。