智能实训-wheeltec小车-抓取(源代码)

语言 :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);//频率70Hz
    while(ros::ok())
    {
        //发布机械臂关节姿态信息，用于控制机械臂
        sensor_