dobot示教程序ros

ros dobot示教程序

Dobot Magician API接口说明V1.2.2版本
1.9HHT功能：

HHT(Hand-hold Teaching)即手持示教。默认情况下,用户按住小臂的圆形解锁按钮,可拖动机械臂到任意位置,松开按钮就可以自动保存一个存点。
1.9.1设置手持示教触发模式
原型 int SetHHTTrigMode (HHTTrigMode hhtTrigMode)
功能 设置手持示教信号触发模式。如果不调用该函数,则默认按键释放时触发
参数 HHTTrigMode:
typedef enum tagHHTTrigMode{
TriggeredOnKeyReleased,		//按键释放时触发
TriggeredOnPeriodicInterval		//按键被按下的过程中触发
}HHTTrigMode;
hhtTrigMode:HHTTrigMode 枚举
返回
DobotCommunicate_NoError:指令正常返回
DobotCommunicate_Timeout:指令无返回,导致超时
获取触发模式
1.9.2获取手持示教触发模式
原型: int GetHHTTrigMode (HHTTrigMode *hhtTrigMode)
功能: 获取手持示教信号触发模式
参数: typedef enum tagHHTTrigMode{
		TriggeredOnKeyReleased,		//按键释放时触发
		TriggeredOnPeriodicInterval		//按键被按下的过程中定时触发
}HHTTrigMode;
hhtTrigMode:HHTTrigMode 枚举
返回:
DobotCommunicate_NoError:指令正常返回
DobotCommunicate_Timeout:指令无返回,导致超时
1.9.3设置手持示教使能状态
原型: int SetHHTTrigOutputEnabled (bool isEnabled)
功能: 设置手持示教状态
参数: isEnabled:0,去使能。1,使能
返回: DobotCommunicate_NoError:指令正常返回
DobotCommunicate_Timeout:指令无返回,导致超时
1.9.4获取手持示教功能使能状态
原型: int GetHHTTrigOutputEnabled (bool *isEnabled)
功能: 获取手持示教使能状态
参数: isEnabled:0,去使能。1,使能
返回 DobotCommunicate_NoError:指令正常返回
DobotCommunicate_Timeout:指令无返回,导致超时
1.9.5获取手持示教触发信号
原型 int GetHHTTrigOutput(bool *isTriggered)
功能 获取手持示教信号。需调用 SetHHTTrigOutputEnabled 接口后才能使用
参数 isTriggered:0,未触发。1,已触发
返回 DobotCommunicate_NoError:指令正常返回
DobotCommunicate_Timeout:指令无返回,导致超时

由该文档可以得到dobot拖动示教的api，并且给了示例程序，我们只需要按着写就可以了。

vector<Pose> position_teach[10];
//获取示教线程
pthread_t teach_tid;
void *teach_fn(void* arg)
{
    int ret=*(int *)arg;
	Pose pose;
	bool trigered=false;
    while (ros::ok())
    {
        /* code for loop body */
        GetHHTTrigOutput(&trigered);
        if(trigered){
            GetPose(&pose);
			position_teach[ret].push_back(pose);
            //ROS_INFO("X: %f Y: %f Z: %f ",pose.x,pose.y,pose.z);
        }
    }
    return NULL;
}

//示教服务程序
int teach_count=0;
uint8_t teach_state=0;
bool Dobot_controller::teach__deal(oryxbot_msgs::standard_mode::Request &req,
									oryxbot_msgs::standard_mode::Response &res)
{
    ROS_INFO("is ok");
	if(req.mode==1&&teach_state==0)		//开始示教
	{
		//示教模式选择
		SetHHTTrigMode(TriggeredOnPeriodicInterval);
		SetHHTTrigOutputEnabled(true);
		int err;
		//这里开启了一个线程用来获取示教点
		err=pthread_create(&teach_tid,NULL,teach_fn,&teach_count);
      	if (err!=0){
			cout << "Error:unable to create thread," << err << endl;
			exit(-1);
      	}else{
			teach_count++;
			teach_state=1;
		}
	}else if(req.mode==0&&teach_state==1)	//关闭示教
	{
		pthread_cancel(teach_tid);//关闭线程
		pthread_join(teach_tid,NULL);//等待线程退出
		ROS_INFO("TEACH FN IS EXIR :\n");
    	SetHHTTrigOutputEnabled(false);
		teach_state=0;
	}else if(req.mode==2)	//执行示教点
	{
		vector<Pose>::iterator pose_temp;
		for(pose_temp=position_teach[req.number].begin();pose_temp<position_teach[req.number].end();++pose_temp)
		{
			vector<float> set_position;
			set_position.push_back((*pose_temp).x);
			set_position.push_back((*pose_temp).y);
			set_position.push_back((*pose_temp).z);
			set_position.push_back((*pose_temp).r);
			ROS_INFO("X: %f Y: %f Z: %f ",set_position[0],set_position[1],set_position[2]);

			if(!set_robot_pose(set_position)) {
				break;
			}
			//下面这个while并没有什么卵用，只是判断到达最终位置了没有，或者到达不了了。
			Pose current_position;	
			ros::Rate rate(100);
			uint64_t i=0;
			float data[3]={0};
			while(ros::ok()){
				i++;
				if(get_robot_pose(&current_position)){
					float a = fabs(current_position.x - set_position[0]);
					float b = fabs(current_position.y - set_position[1]);
					float c = fabs(current_position.z - set_position[2]);
					float d = fabs(current_position.r - set_position[3]);
					if(i%50==0){
						float line=pow(data[0]-a,2)+pow(data[1]-b,2)+pow(data[2]-c,2);
						if(line<10){break;}		//长时间（0.5s）未移动退出
						data[0]=a;
						data[1]=b;
						data[2]=c;
					}
					if(a<0.05 && b<0.05 && c<0.05 && d<0.05){
						break;	
					}
				}
			}
		}
	}
	res.success=true;
	return true;
}

具体实现程序如上，服务初始化如下：

ros::ServiceServer teach_server = m_handle.advertiseService("teach",&Dobot_controller::teach__deal,this);

用到的服务standard_mode.srv

uint8 number	#mode=2的情况下开始第 number 次示教程序
uint8 mode		#mode 0-结束示教；1-开始示教；2-执行示教
---
bool success
string message

程序只给出了一部分，dobot初始化等可以自行实现。