引言：使用飞控的主通道5-8作为舵机控制输出，需要提前设置好飞控参数，否则无效。本节资料文件尚未整理完毕，整理完毕后会在B站进行视频讲解，并进行开源
1、启动mavros通信，用于订阅遥控器的按键信息，添加机械爪是否使用的安全开关
javascript
roslaunch robot_bringup px4.launch2、启动D435深度摄像头，启动深度与彩图对齐文件
注：先查看深度摄像头是否工作正常，采用USB3.0线
javascript
roslaunch realsense2_camera rs_aligned_depth.launch 3、启动yolov8物体识别节点
javascript
roslaunch yolov8_ros yolo_v8_d435.launch4、启动坐标转换节点
javascript
roslaunch object_position object_position.launch 5、启动机械爪控制节点
javascript
rosrun arm_grasp arm_grasp机械爪控制代码如下：
javascript
//说明：舵机方向根据实际调整，这里用的占空比0%表示闭合，100%表示打开
#include <api_library.h>
#include <lib_library.h>
#include <arm_grasp/ctrl.h>
#include <mavros_msgs/RCIn.h>
int arm_first = 0;
int arm_second = 0;
int arm_third = 0;
int arm_fourth = 0;
bool target_locked_flag = false;
int target_locked_area = 100;
arm_grasp::ctrl ctrl_msg;
//无人机遥控器订阅
mavros_msgs::RCIn global_rc_in;
void rc_in_cb(const mavros_msgs::RCIn::ConstPtr& msg)
{
global_rc_in = *msg;
if(global_rc_in.channels[6]>=1250&&global_rc_in.channels[6]<=2050)
{
ctrl_msg.data = true;
}
else
{
ctrl_msg.data = false;
}
}
/**************************************************
//此处添加计数器防止误识别判断
//补充说明，此处采用摄像头的左上前坐标系，解释如下：
//X为负，表示物体在摄像头的左方，反之右方，正中间为0；
//Y为负，表示物体在摄像头的前方，反之后 方，正中间为0；
//Z表示距离，始终为正
***************************************************/
int last_count = 0;
int current_count = 0;
int last_diff = 0;
int current_diff = 0;
int sum = 0;
int visual_position(string str)
{
if(object_pos.header.frame_id == str)
{
position_detec_x = object_pos.point.x;
position_detec_y = object_pos.point.y;
position_detec_z = object_pos.point.z;
}
if((fabs(position_detec_x)<=0.03)&&position_detec_y<=-0.03)
{
current_count= current_count + 1;
// cout<<"目标在正前方"<<endl;
}
else if((fabs(position_detec_x)<=0.03)&&position_detec_y>=0.03)
{
current_count= current_count + 2;
// cout<<"目标在正后方"<<endl;
}
else if((fabs(position_detec_y)<=0.03)&&position_detec_x<=-0.03)
{
current_count= current_count + 3;
// cout<<"目标在正左方"<<endl;
}
else if((fabs(position_detec_y)<=0.03)&&position_detec_x>=0.03)
{
current_count= current_count + 4;
// cout<<"目标在正右方"<<endl;
}
else if((position_detec_x<-0.03)&&position_detec_y<-0.03)
{
current_count= current_count + 5;
// cout<<"目标在左前方"<<endl;
}
else if((position_detec_x<-0.03)&&position_detec_y>0.03)
{
current_count= current_count + 6;
// cout<<"目标在左后方"<<endl;
}
else if((position_detec_x>0.03)&&position_detec_y<-0.03)
{
current_count= current_count + 7;
// cout<<"目标在右前方"<<endl;
}
else if((position_detec_x>0.03)&&position_detec_y>0.03)
{
current_count= current_count + 8;
// cout<<"目标在右后方"<<endl;
}
else if((fabs(position_detec_x)<=0.03)&&fabs(position_detec_y)<=0.03&&position_detec_x!=0&&position_detec_y!=0)
{
current_count= current_count + 9;
// cout<<"目标在正中间"<<endl;
}
else
{
}
//cout<<current_count<<endl;
//cout<<last_count<<endl;
if(current_count ==last_count&&last_count!=0)
{
sum++;
if(sum>=100)
{
cout<<"目标锁定"<<endl;
target_locked_area = current_count;
cout<<target_locked_area<<endl;
target_locked_flag = true;
}
}
else
{
sum = 0;
}
last_count=current_count;
current_count = 0;
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "complete_mission");
ros::NodeHandle nh;
ros::Subscriber object_pos_sub = nh.subscribe<geometry_msgs::PointStamped>("object_position", 100, object_pos_cb);
ros::Subscriber rc_in_sub = nh.subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 100, rc_in_cb);
ros::ServiceClient ctrl_pwm_client = nh.serviceClient<mavros_msgs::CommandLong>("mavros/cmd/command");
ros::Time last_request = ros::Time::now();
lib_pwm_control(50,50,50,50);
ctrl_pwm_client.call(lib_ctrl_pwm);
ctrl_msg.data = false;
ros::Rate rate(20);
while(ros::ok())
{
visual_position("cylinder");
//cout<<ctrl_msg.data<<endl;
if(ctrl_msg.data)
{
cout<<target_locked_area<<endl;
//正前方
if(target_locked_area==1||target_locked_area==5||target_locked_area==7)
{
arm_first = 100;
arm_second = 0;
}
else if(target_locked_area==2||target_locked_area==6||target_locked_area==8)
{
arm_first = 0;
arm_second = 100;
}
//中间
else
{
arm_first = 50;
arm_second = 50;
}
lib_pwm_control(arm_first,arm_second,arm_third,arm_fourth);
ctrl_pwm_client.call(lib_ctrl_pwm);
if(lib_time_record_func(5.0,ros::Time::now()))
{
arm_fourth = 100;
ROS_INFO("执行抓取");
}
}
else
{
lib_pwm_control(100,50,50,50);
ctrl_pwm_client.call(lib_ctrl_pwm);
ROS_INFO("机械爪还原");
}
ros::spinOnce();
rate.sleep();
}
return 0;
}