引言:使用飞控的主通道5-8作为舵机控制输出,需要提前设置好飞控参数,否则无效。本节资料文件尚未整理完毕,整理完毕后会在B站进行视频讲解,并进行开源
1、启动mavros通信,用于订阅遥控器的按键信息,添加机械爪是否使用的安全开关
roslaunch robot_bringup px4.launch
2、启动D435深度摄像头,启动深度与彩图对齐文件
注:先查看深度摄像头是否工作正常,采用USB3.0线
roslaunch realsense2_camera rs_aligned_depth.launch
3、启动yolov8物体识别节点
roslaunch yolov8_ros yolo_v8_d435.launch
4、启动坐标转换节点
roslaunch object_position object_position.launch
5、启动机械爪控制节点
rosrun arm_grasp arm_grasp
机械爪控制代码如下:
#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;}
}
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;}else if((fabs(position_detec_x)<=0.03)&&position_detec_y>=0.03){current_count= current_count + 2;}else if((fabs(position_detec_y)<=0.03)&&position_detec_x<=-0.03){current_count= current_count + 3;}else if((fabs(position_detec_y)<=0.03)&&position_detec_x>=0.03){ current_count= current_count + 4;}else if((position_detec_x<-0.03)&&position_detec_y<-0.03){current_count= current_count + 5;}else if((position_detec_x<-0.03)&&position_detec_y>0.03){current_count= current_count + 6;}else if((position_detec_x>0.03)&&position_detec_y<-0.03){current_count= current_count + 7;}else if((position_detec_x>0.03)&&position_detec_y>0.03){current_count= current_count + 8; }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;}else{}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");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;
}