语言 :C

源代码：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>#include </usr/include/opencv2/highgui/highgui_c.h> //树莓派(Raspberry)、工控机(IPC)、虚拟机
//#include </usr/include/opencv4/opencv2/highgui/highgui_c.h> //jetson Nano/NX/TX2
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>#include <dynamic_reconfigure/server.h>
#include <stepper_arm/arm_color_block_paramsConfig.h>#include <pthread.h>
#include <arm_2_control.h>using namespace std;
using namespace cv;//ROS图片话题
string rgb_image_topic;
//opencv图片对象
Mat image, HSV_image, final_image;
//ROS里程计话题
string odom_topic;/****可以使用rqt动态调参的变量****/
//抓取色块标志位
int Pick_start=false;
double car_search_distance_max=3;
//机械臂初始位置
double grab_start_jointA = 1.570796;
double grab_start_position_x = 0.062;
double grab_start_position_y =-0.008;
//机械臂结束位置
double grab_end_jointA = -1.570796;
double grab_end_position_x = 0.062;
double grab_end_position_y = 0.062;//机械爪初始张开程度
double hand_open_size=-0.7; 
//机械爪抓取色块夹紧程度
double hand_close_size=0.5; 
//色块识别阈值
int color=0;//0:dynamic, 1:green, 2:blue, 3:yellow. 4:red
int dynamic_hsv[6]={0,  0,   0,   0,   0,   0  }; //[h_min, s_min, v_min, h_max, s_max, v_max]
int green_hsv  [6]={60, 35,  0,   80,  180, 255}; //[h_min, s_min, v_min, h_max, s_max, v_max]
int blue_hsv   [6]={80, 90,  100, 130, 200, 255}; //[h_min, s_min, v_min, h_max, s_max, v_max]
int yellow_hsv [6]={15, 110, 30,  50,  255, 255}; //[h_min, s_min, v_min, h_max, s_max, v_max]
int red_hsv    [6]={0,  100, 53,  18,  223, 255}; //[h_min, s_min, v_min, h_max, s_max, v_max]
//膨胀腐蚀大小
int dilate_erode_size=7;
//期望识别到的目标的面积的大小，代表距离。因为RGB相机没有深度信息，只能在已知目标大小的情况下，通过判断面积大小计算近似距离(近大远小)
double target_areaSize=22000; 
//期望色块位于画面中心向上偏移多少像素点
double target_y_bias=0; 
//寻找色块时小车的转速
double car_search_turn=0.1;
double car_search_foward=0.1;
//抓取色块时机械臂的运动限幅参数，前进后退、上升下降、左转右转
float Move_step_max=0.4, Turn_step_max=0.2;
float Move_step_min=0.0005, Turn_step_min=0.0005;
//抓取色块时机械臂运动靠近色块的PID参数
double turn_KP=0.05,   turn_KD=0;
double down_KP=0.01,   down_KD=0;
double foward_KP=0.01, foward_KD=0;
//寻找色块时小车的运动参数,
double Car_foward_KP;
/****可以使用rqt动态调参的变量****///机械臂小车寻找抓取色块的相关变量
double area;
double distance_bias, x_bias, y_bias;
double last_distance_bias, last_x_bias, last_y_bias;
double distance_step=0, x_step=0, y_step=0;
double foward_step, down_step;
double Car_foward_velocity, Car_turn_velocity;
bool foward_forb=false;
//机械臂小车寻找抓取色块过程中的各个状态变量
bool Car_stop=false, car_turn_locked=false, Arm_stop=true;
bool hand_on_color=false, arm_turn_locked=false, arm_locked=false, hand_closed=false;
int Car_turn_times=0;
//小车里程计，用于寻找色块功能
double odom_foward, odom_lateral, odom_turn;
//机械臂时间状态变量
ros::Time arm_position_move_time, arm_locked_time, hand_closed_time;//rqt动态调参函数
void dynamic_reconfigure_callback(stepper_arm::arm_color_block_paramsConfig &config, uint32_t level);
//获取摄像头图像后的处理函数，主要功能实现在这里
void rgb_image_Callback(const sensor_msgs::ImageConstPtr& msg);
//获得小车里程计
void odom_Callback(const nav_msgs::Odometry& msg);
//opencv进度条调阈值回调函数，放弃使用，改由rqt调动态阈值
void on_trackbar(int, void*) {}/**************************************************************************
Function: The main function
Input   : none
Output  : 无
函数功能： 主函数
入口参数： 无
返回  值： 无
**************************************************************************/
int main(int argc, char **argv)
{ros::init(argc,argv,"arm_2_auto_pick_colorBlock"); //初始化节点ros::AsyncSpinner spinner(1); spinner.start();ros::NodeHandle NodeHandle; //创建节点句柄//关节B默认角度(相对X轴，逆时针为正)NodeHandle.param<double>("Angle_B_bias",    Angle_B_bias,    1.571);//关节C默认角度(相对关节B，逆时针为正)NodeHandle.param<double>("Angle_C_bias",    Angle_C_bias,    0.384);//机械臂底座到地面的距离NodeHandle.param<double>("arm_base_height", arm_base_height, 0);//图片话题名NodeHandle.param<string>("rgb_image_topic", rgb_image_topic, "/usb_cam/image_raw");//里程计话题名NodeHandle.param<string>("odom_topic",      odom_topic,      "/odom");//摄像头图片话题订阅者，用于识别抓取色块ros::Subscriber rgb_image_Sub; rgb_image_Sub   = NodeHandle.subscribe(rgb_image_topic, 20, &rgb_image_Callback);//机械臂关节姿态信息发布者，用于控制机械臂ros::Publisher Arm_2_JointStates_Pub;Arm_2_JointStates_Pub = NodeHandle.advertise<sensor_msgs::JointState>("Arm_2_JointStates", 10);//速度命令话题发布者，用于控制小车运动寻找色块ros::Publisher cmd_vel_Pub;cmd_vel_Pub = NodeHandle.advertise<geometry_msgs::Twist>("cmd_vel_ori", 10);//里程计话题订阅者，用于识别抓取色块ros::Subscriber odom_Sub; odom_Sub   = NodeHandle.subscribe(odom_topic, 20, &odom_Callback);//动态调参初始化，用于控制抓取色块、改变目标色块颜色dynamic_reconfigure::Server<stepper_arm::arm_color_block_paramsConfig> server;dynamic_reconfigure::Server<stepper_arm::arm_color_block_paramsConfig>::CallbackType f;f = boost::bind(&dynamic_reconfigure_callback, _1, _2);server.setCallback(f);Joint_A=grab_start_jointA;ArmC_End_Position_X=grab_start_position_x;ArmC_End_Position_Y=grab_start_position_y;Joint_Hand_left1  =  hand_open_size, Joint_Hand_left2  =  hand_open_size, Joint_Hand_right1 = -hand_open_size, Joint_Hand_right2 = -hand_open_size;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);ros::Rate loopRate(70);//频率70Hzwhile(ros::ok()){//发布机械臂关节姿态信息，用于控制机械臂sensor_msgs::JointState Arm_2_JointStates;Arm_2_JointStates.position.push_back(Joint_A);//减去Angle_B_bias是因为moveit的关节初始位置为0Arm_2_JointStates.position.push_back(Joint_B-Angle_B_bias);//减去Angle_C_bias是因为moveit的关节初始位置为0Arm_2_JointStates.position.push_back(Joint_C-Angle_C_bias);Arm_2_JointStates.position.push_back(Joint_End);Arm_2_JointStates.position.push_back(Joint_Hand_left1);Arm_2_JointStates.position.push_back(Joint_Hand_left2);Arm_2_JointStates.position.push_back(Joint_Hand_right1);Arm_2_JointStates.position.push_back(Joint_Hand_right2);Arm_2_JointStates.position.push_back(Joint_Grasper);Arm_2_JointStates_Pub.publish(Arm_2_JointStates);   //发布速度命令话题，用于控制小车运动寻找色块geometry_msgs::Twist cmd_vel;cmd_vel.linear.x =Car_foward_velocity;cmd_vel.linear.y =0;cmd_vel.linear.z =0;cmd_vel.angular.x=0;cmd_vel.angular.y=0;cmd_vel.angular.z=Car_turn_velocity;cmd_vel_Pub.publish(cmd_vel); ros::spinOnce();loopRate.sleep();} return 0;
}/**************************************************************************
Function: rqt dynamically calls the parameter callback function
Input   : none
Output  : none
函数功能： rqt动态调参回调函数
入口参数： 动态调参对象，
返回  值： 无
**************************************************************************/
void dynamic_reconfigure_callback(stepper_arm::arm_color_block_paramsConfig &config, uint32_t level)
{Pick_start=config.Pick_start;car_search_distance_max=config.car_search_distance_max;grab_start_jointA=config.grab_start_jointA;grab_start_position_x=config.grab_start_position_x;grab_start_position_y=config.grab_start_position_y;grab_end_jointA=config.grab_end_jointA;grab_end_position_x=config.grab_end_position_x;grab_end_position_y=config.grab_end_position_y;hand_open_size=config.hand_open_size;hand_close_size=config.hand_close_size;color=config.color;//使用ROS的rqt工具进行动态调阈值dynamic_hsv[0]=config.HSV_H_MIN;dynamic_hsv[1]=config.HSV_S_MIN;dynamic_hsv[2]=config.HSV_V_MIN;dynamic_hsv[3]=config.HSV_H_MAX;dynamic_hsv[4]=config.HSV_S_MAX;dynamic_hsv[5]=config.HSV_V_MAX;dilate_erode_size=config.dilate_erode_size;/*cout<<"color= "<<color<<endl;cout<<"levle:"<<level<<endl;*/car_search_foward=config.car_search_foward;target_areaSize=config.target_areaSize;target_y_bias=config.target_y_bias;Move_step_max=config.Move_step_max;Turn_step_max=config.Turn_step_max;Move_step_min=config.Move_step_min;Turn_step_min=config.Turn_step_min;turn_KP=config.turn_KP;turn_KD=config.turn_KD;down_KP=config.down_KP;down_KD=config.down_KD;foward_KP=config.foward_KP;foward_KD=config.foward_KD;Car_foward_KP=config.Car_foward_KP;
}/**************************************************************************
Function: RGB image topic subscription callback function
Input   : none
Output  : none
函数功能： RGB图像话题订阅回调函数
入口参数： 无
返回  值： 无
**************************************************************************/
void rgb_image_Callback(const sensor_msgs::ImageConstPtr& msg)
{  /*零、ROS图片话题转换为OpenCV图片格式*************************/try {cv_bridge::CvImagePtr cv_ptr;  //ROS图像格式//将ROS话题数据转换为ROS图像格式cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);//将ROS图像格式转换为opencv图像格式cv_ptr->image.copyTo(image);//压缩图片，提高图片处理速度，同时减轻远程运行程序时的网络带宽压力CvSize size;size.width = image.cols/2;size.height = image.rows/2;resize(image,image,size,0,0, CV_INTER_AREA);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}/*零、ROS图片话题转换为OpenCV图片格式*************************//*一、图像处理：识别色块部分**********************************/cvtColor(image, HSV_image, CV_BGR2HSV);//RGB图像转换为HSV图像/*0.在原图上标记出目标位置*/double target_x=image.cols/2, target_y=target_y_bias; //控制机械臂使色块位于画面内的目标位置circle(image, Point(target_x, target_y), 10, Scalar(0, 0, 255), 1); //画色块轮廓外接圆    line(image, Point(target_x-10,target_y),Point(target_x+10,target_y), Scalar(0, 0, 255),1,8,0);//在目标位置画十字line(image, Point(target_x,target_y-10),Point(target_x,target_y+10), Scalar(0, 0, 255),1,8,0);//在目标位置画十字/*1.根据颜色阈值进行图片二值化*/if(color==0) //根据HSV颜色空间下的自定义颜色阈值(即动态调阈值)，进行图像二值化，仅保留指定自定义颜色{inRange(HSV_image, Scalar(dynamic_hsv[0], dynamic_hsv[1], dynamic_hsv[2]), Scalar(dynamic_hsv[3], dynamic_hsv[4], dynamic_hsv[5]), final_image); }else{if(color==1) //根据HSV颜色空间下的预置绿色阈值，进行图像二值化，仅保留绿色{inRange(HSV_image, Scalar(green_hsv[0], green_hsv[1], green_hsv[2]), Scalar(green_hsv[3], green_hsv[4], green_hsv[5]), final_image); }if(color==2) //根据HSV颜色空间下的预置蓝色阈值，进行图像二值化，仅保留蓝色{inRange(HSV_image, Scalar(blue_hsv[0], blue_hsv[1], blue_hsv[2]), Scalar(blue_hsv[3], blue_hsv[4], blue_hsv[5]), final_image); }if(color==3) //根据HSV颜色空间下的预置黄色阈值，进行图像二值化，仅保留黄色{inRange(HSV_image, Scalar(yellow_hsv[0], yellow_hsv[1], yellow_hsv[2]), Scalar(yellow_hsv[3], yellow_hsv[4], yellow_hsv[5]), final_image); }      if(color==4) //根据HSV颜色空间下的预置红色阈值，进行图像二值化，仅保留红色{inRange(HSV_image, Scalar(red_hsv[0], red_hsv[1], red_hsv[2]), Scalar(red_hsv[3], red_hsv[4], red_hsv[5]), final_image); }}/*2.膨胀腐蚀清除图像杂质*/Mat element = getStructuringElement(MORPH_RECT, Size(dilate_erode_size, dilate_erode_size)); dilate(final_image, final_image, element);erode(final_image, final_image, element);erode(final_image, final_image, element);dilate(final_image, final_image, element);//3.获得当前图像存在的所有轮廓(经过前面的处理，只有指定颜色的目标(色块)才有轮廓)vector<vector<Point>> contours;vector<Vec4i> hierarchy; try{findContours(final_image,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE,Point());}catch (cv::Exception& e){const char* err_msg = e.what();cout << "exception caught: " << err_msg << endl;}/*一、图像处理：识别色块部分**********************************//*********如果视野内存在色块，则停止色块寻找，开始执行色块抓取*//*********如果视野内存在色块，则停止色块寻找，开始执行色块抓取*//*********如果视野内存在色块，则停止色块寻找，开始执行色块抓取*//*二、图像处理：提取色块位置信息与可视化部分*******************/static int object_found=0;if(contours.size()>0) //如果轮廓对象不为空，则开始进行处理{if(object_found==0)cout<<GREEN<<"Object found."<<RESET<<endl<<endl; //只在第一次发现目标色块时打印object_found=1;int largest_contours_order=0; //所有轮廓中面积最大轮廓的序号/*1.提取色块位置信息*///获得面积最大轮廓的序号double largest_area=0;for(int i=0;i<contours.size();i++)  {  if(cv::contourArea(contours[i])>largest_area){largest_area=cv::contourArea(contours[i]);largest_contours_order=i;}      }//获得面积最大轮廓的面积大小area= cv::contourArea(contours[largest_contours_order]);//cout<<"area: "<<area<<endl; //面积最大轮廓的外接圆半径float radius=0; //面积最大轮廓的中心位置(横轴为x，纵轴为y，右上角为原点)Point2f center;//求面积最大轮廓外接圆，获得轮廓中心位置minEnclosingCircle(contours[largest_contours_order], center, radius);  //求获得当前色块与目标位置、距离的偏差x_bias=image.cols/2-center.x; //横轴偏差，色块中心在图像中心的左边为正，单位为分辨率y_bias=image.rows/2-center.y-target_y; //纵轴偏差，色块中心在目标位置的上边为正，单位为分辨率distance_bias=target_areaSize-area; //目标面积-色块面积=距离偏差，色块过远为正，单位为分辨率*分辨率/*cout<<"distance_bias: "<<distance_bias<<endl; cout<<"x_bias: "<<x_bias<<endl; cout<<"y_bias: "<<y_bias<<endl;*/ /*2.色块位置信息可视化*//*2.1.1在原图上标记出识别到的指定颜色目标*/circle(image, Point(center.x, center.y), radius, Scalar(0, 0, 255), 5); //画色块轮廓外接圆    line(image, Point(center.x-10,center.y),Point(center.x+10,center.y), Scalar(0, 0, 255),1,8,0);//在色块中心画十字line(image, Point(center.x,center.y-10),Point(center.x,center.y+10), Scalar(0, 0, 255),1,8,0);//在色块中心画十字/*2.2.在原图上显示当前色块与目标位置(摄像头中心)、距离的偏差*///定义用于显示的文字变量string area_txt="area: ";string bias_area_txt="bias_area: ";string bias_x_txt="bias_x: ";string bias_y_txt="bias_y: ";//初始化用于显示文字的字体格式CvFont font;double hScale=0.5, vScale=0.5;int    lineWidth=1.5;cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,hScale,vScale,0,lineWidth,8);//限制要显示的文字在图像上的位置在图像大小内int txt_position_x, txt_position_y;txt_position_x=10;txt_position_y=50;if(txt_position_x<5)txt_position_x=5;if(txt_position_x>image.cols-50)txt_position_x=image.cols-50;if(txt_position_y<50)txt_position_y=50;if(txt_position_y>image.rows-60)txt_position_y=image.rows-60;//显示文字需要先转换图像格式IplImage tmp = IplImage(image);CvArr* image_=(CvArr*)&tmp;//显示当前色块面积area_txt = area_txt + to_string((int)area);cvPutText(image_, area_txt.data(), cvPoint(txt_position_x, txt_position_y-30), &font, cvScalar(0,0,0,1));//显示目标色块面积与当前色块面积的偏差bias_area_txt = bias_area_txt + to_string((int)distance_bias);cvPutText(image_, bias_area_txt.data(), cvPoint(txt_position_x, txt_position_y-10), &font, cvScalar(0,0,0,1));//显示图像中心与当前色块中心的横轴偏差bias_x_txt = bias_x_txt + to_string((int)x_bias);cvPutText(image_, bias_x_txt.data(), cvPoint(txt_position_x, txt_position_y+10), &font, cvScalar(0,0,0,1));//显示图像中心与当前色块中心的纵轴偏差bias_y_txt = bias_y_txt + to_string((int)y_bias);cvPutText(image_, bias_y_txt.data(), cvPoint(txt_position_x, txt_position_y+30), &font, cvScalar(0,0,0,1));//转换图像格式image = cv::cvarrToMat(image_);/*二、图像处理：提取色块位置信息与可视化部分*******************//*三、如果发现了色块，首先控制小车移动，使小车移动到与色块合适的距离，然后再计算机械臂的运动数值，用于抓取色块；如果当前视野内没有发现色块，则控制小车运动寻找色块*//*1.首先控制小车移动，使小车移动到与色块合适的距离*/if(Car_stop==false && Pick_start==true){/*1.1.如果横轴偏差比较大，控制小车旋转，使偏差减小到0*/if(x_bias>70 || x_bias<-70){Car_foward_velocity=x_bias/400*Car_foward_KP;}else{Car_foward_velocity=0;Car_stop=true, Arm_stop=false;cout<<GREEN<<"Car location complete."<<RESET<<endl<<endl;}}/*2.计算机械臂的运动数值，用于抓取色块*/if(Car_stop==true && Arm_stop==false && arm_locked==false){/*2.1.如果距离偏差(色块与机械臂的距离)大于1000，则对y轴偏差做处理，求机械臂上下方向运动值*/if((distance_bias>1000)&& arm_locked==false){//如果偏差较大，说明机械爪还没有靠近目标，继续运动进行调整y_step = (y_bias*down_KP + (y_bias-last_y_bias)*down_KD)*0.0005;  }/*2.2.如果距离偏差(色块与机械臂的距离)小于1000，则锁定机械臂的前后上下方向的位移。下一步机械爪将夹紧*/else if(arm_locked==false){y_step = 0;arm_locked=true;arm_locked_time=ros::Time::now();cout<<GREEN<<"Arm_hand can close now."<<RESET<<endl<<endl;}//if(area<target_areaSize*0.8 && y_step<0.00 && ArmC_End_Position_X<0.145 &&arm_in_min_height==1) y_step=distance_step*1; //机械爪离目标远时，禁止机械臂后退/*2.3.如果横轴偏差大小大于5，则对偏差做处理，求机械臂左右旋转的运动值*/if((x_bias>5 || x_bias<-5  )&& arm_turn_locked==false){x_step = (x_bias*turn_KP + (x_bias-last_distance_bias)*turn_KD)*0.01;      }else {x_step = 0;//当机械臂靠近色块后，转向偏差的认为已经稳定，后续禁止转向运动if(distance_bias<5000 && distance_bias>-5000) arm_turn_locked=true;}/*2.4.限制机械臂前后左右上下方向运动值的最大值和最小值*///前后上下移动的最大值(单位：m)，转向移动的最大值(单位：rad)          if(x_step> Turn_step_max)       x_step= Turn_step_max;if(x_step<-Turn_step_max)       x_step=-Turn_step_max;if(y_step> Move_step_max)       y_step= Move_step_max;if(y_step<-Move_step_max)       y_step=-Move_step_max;if(distance_step> Move_step_max)distance_step= Move_step_max;if(distance_step<-Turn_step_max)distance_step=-Turn_step_max;//前后上下移动的最小值(单位：m)，转向移动的最小值(单位：rad)if(x_step< Turn_step_min && x_step> 0)x_step= Turn_step_min;if(x_step>-Turn_step_min && x_step< 0)x_step=-Turn_step_min;             if(y_step< Move_step_min && y_step> 0)y_step= Move_step_min;if(y_step>-Move_step_min && y_step< 0)y_step=-Move_step_min;if(distance_step< Move_step_min && distance_step> 0)distance_step= Move_step_min;   //机械臂不允许向上移动，提高机械臂向色块靠近的稳定性if(distance_step<0)distance_step=0;/*cout<<"distance_step: "<<distance_step<<endl; cout<<"x_step: "<<x_step<<endl; cout<<"y_step: "<<y_step<<endl; *///用于KD控制last_distance_bias=distance_bias;last_x_bias=x_bias;last_y_bias=y_bias;}      }/*******如果轮廓对象为空，则说明当前视野内没有目标颜色，执行寻找色块功能*******/else //对应判断if(contours.size()>0) {/*1.机械臂前后左右上下方向运动值置0，即停止色块抓取*/distance_step=0;x_step=0;y_step=0;Joint_Hand_left1  =  hand_open_size, Joint_Hand_left2  =  hand_open_size, Joint_Hand_right1 = -hand_open_size, Joint_Hand_right2 = -hand_open_size;object_found=0;cout<<RED<<"no object found."<<RESET<<endl;   /*2.寻找色块*/  if(Pick_start==true && Arm_stop==true && foward_forb==false)//判断是否下达了抓取色块命令{          Car_foward_velocity=car_search_foward;}else {Car_foward_velocity=0;Car_turn_velocity=0;}    }/*三、如果发现了色块，首先控制小车移动，使小车移动到与色块合适的距离，然后再计算机械臂的运动数值，用于抓取色块；如果当前视野内没有发现色块，则控制小车运动寻找色块*//*四、根据机械臂前后左右上下方向运动值决定机械臂应该执行什么动作*//*1.1.机械臂前后上下方向运动值不为0，则控制机械臂靠近色块*/if(Arm_stop==false && arm_locked==false){//cout<<"distance_step!=0 && y_step!=0"<<endl;int error_foward, error_down;     error_foward=Position_move(foward, 0.01*foward_KP);/*cout<<"error_foward: "<<error_foward<<endl;cout<<"error_foward&(1<<5): "<<(error_foward&(1<<5))<<endl;cout<<"error_foward&(1<<3): "<<(error_foward&(1<<3))<<endl<<endl;*/error_down=Position_move(down, -y_step);/*cout<<"error_down: "<<error_down<<endl;cout<<"error_down&(1<<5): "<<(error_down&(1<<5))<<endl; cout<<"error_down&(1<<3): "<<(error_down&(1<<3))<<endl<<endl; */}/*1.2.机械臂左右方向运动值不为0，则控制机械臂旋转*/if(x_step!=0){//cout<<"x_step!=0"<<endl;Joint_A = Joint_A + x_step;}/*2.1.如果机械爪锁定标志位置1，代表已经抓取到色块，控制机械臂返回初始位置*/if(hand_closed==true){//机械爪之前色块后 if(( ros::Time::now() - hand_closed_time).toSec()<2.0){//0-2秒：机械臂回到默认位置ArmC_End_Position_X=grab_start_position_x;ArmC_End_Position_Y=grab_start_position_y;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}else if(( ros::Time::now() - hand_closed_time).toSec()<5.0){//2-5秒：机械臂底座旋转到终点Joint_A=grab_end_jointA;}else if(( ros::Time::now() - hand_closed_time).toSec()<6.0){//5-6秒：机械臂前进到目标点X坐标ArmC_End_Position_X=grab_end_position_x;ArmC_End_Position_Y=grab_start_position_y;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}else if(( ros::Time::now() - hand_closed_time).toSec()<7.0){//6-7秒：机械臂下降到目标点Y坐标(地面)ArmC_End_Position_X=grab_end_position_x;ArmC_End_Position_Y=grab_end_position_y;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}else if(( ros::Time::now() - hand_closed_time).toSec()<8.0){//7-8秒：机械爪张开，放置色块Joint_Hand_left1  =  hand_open_size, Joint_Hand_left2  =  hand_open_size, Joint_Hand_right1 = -hand_open_size, Joint_Hand_right2 = -hand_open_size;}else if(( ros::Time::now() - hand_closed_time).toSec()<9.0){//8-9秒：机械臂上升回去ArmC_End_Position_X=grab_end_position_x;ArmC_End_Position_Y=grab_start_position_y;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}else if(( ros::Time::now() - hand_closed_time).toSec()<12.0){//9-12秒：机械臂直接回归起点Joint_A=grab_start_jointA;ArmC_End_Position_X=grab_start_position_x;ArmC_End_Position_Y=grab_start_position_y;Joint_Hand_left1  =  hand_open_size, Joint_Hand_left2  =  hand_open_size, Joint_Hand_right1 = -hand_open_size, Joint_Hand_right2 = -hand_open_size;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}else if(( ros::Time::now() - hand_closed_time).toSec()>12.0){//回归后，清空相关标志位，为下一次抓取做准备Car_stop=false, Arm_stop=true, Car_turn_times=0;car_turn_locked=false, arm_turn_locked=false, arm_locked=false, hand_closed=false;Car_foward_velocity=0, Car_turn_velocity=0;foward_forb=false;}}/*2.2.如果机械臂锁定标志位置1，代表已经靠近色块，控制机械臂夹紧抓取色块*/if(arm_locked==true && hand_closed==false){/*cout<<"_start_time: "<<arm_locked_time.toSec()<<endl;cout<<"ros::Time::now(): "<<ros::Time::now().toSec()<<endl;*/Joint_Hand_left1  =  hand_close_size;Joint_Hand_left2  =  hand_close_size;Joint_Hand_right1 = -hand_close_size;Joint_Hand_right2 = -hand_close_size;//cout<<"( ros::Time::now() - _start_time).toSec(): "<<( ros::Time::now() - arm_locked_time).toSec()<<endl;//夹紧后等待1秒钟，再置1机械爪锁定标志位if(( ros::Time::now() - arm_locked_time).toSec()>1 && hand_closed==false)//delay 1s{hand_closed=true; hand_closed_time=ros::Time::now();}}/*五、如果抓取色块标志位置0，机械臂状态全部复位*/if(Pick_start==false){Car_stop=false, Arm_stop=true, Car_turn_times=0;car_turn_locked=false, arm_turn_locked=false, arm_locked=false, hand_closed=false;Car_foward_velocity=0, Car_turn_velocity=0;foward_forb=false;Joint_A=grab_start_jointA;ArmC_End_Position_X=grab_start_position_x;ArmC_End_Position_Y=grab_start_position_y;Joint_Hand_left1  =  hand_open_size, Joint_Hand_left2  =  hand_open_size, Joint_Hand_right1 = -hand_open_size, Joint_Hand_right2 = -hand_open_size;arm_2_inverse_solution(ArmC_End_Position_X, ArmC_End_Position_Y, Joint_B, Joint_C);}/*cout<<endl<<GREEN<<"arm_turn_locked: "<<arm_turn_locked<<endl;cout<<GREEN<<"arm_locked: "<<arm_locked<<endl;cout<<GREEN<<"hand_closed: "<<hand_closed<<endl;cout<<GREEN<<"Car_stop: "<<Car_stop<<endl;cout<<GREEN<<"Arm_stop: "<<Arm_stop<<RESET<<endl<<endl;*///显示原始图片，添加色块位置标注CvSize size;size.width = image.cols/2; //再次压缩图片，减轻远程运行程序时的网络带宽压力size.height = image.rows/2;resize(image,image,size,0,0, CV_INTER_AREA);imshow("rgb_image", image);//显示二值化图片，jetsonNano/NX目前无法显示单通道图像，暂时关闭//imshow("final_rgb_image", final_image);waitKey(1);
}/**************************************************************************
Function: Odometer topic subscribe callback function
Input   : none
Output  : none
函数功能： 里程计话题订阅回调函数
入口参数： 无
返回  值： 无
**************************************************************************/
void odom_Callback(const nav_msgs::Odometry& msg)
{static double last_odom_turn, last_odom_foward;odom_foward  = msg.pose.pose.position.x-last_odom_foward; //Position //位置odom_lateral = msg.pose.pose.position.y;odom_turn    = msg.pose.pose.position.z-last_odom_turn;if(Pick_start==false){last_odom_turn = msg.pose.pose.position.z;last_odom_foward = msg.pose.pose.position.x;}   if(hand_closed==true)last_odom_foward = msg.pose.pose.position.x;if(car_search_distance_max>0 && odom_foward>car_search_distance_max)//前进3m还没有发现色块，则停止运动foward_forb=true;if(car_search_distance_max<0 && odom_foward<car_search_distance_max)//前进3m还没有发现色块，则停止运动foward_forb=true;/*cout<<RED<<"odom_Callback"<<endl;cout<<RED<<"odom_foward:  "<<odom_foward<<endl;cout<<RED<<"odom_lateral: "<<odom_lateral<<endl;cout<<RED<<"odom_turn:    "<<odom_turn<<RESET<<endl;*/
}

小车抓取物块时的截图：