一起做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控制。機械臂想要做到實時跟蹤就比較難了，直接控制關節轉動容易導致一些碰撞問題，只能儘量採用速度更快的軌跡求解方法。