Darknet+ros+realsenseD435i+yolo(ubuntu20.04)

一、下载Darknet_ros

mkidr -p yolo_ws/src
cd yolo_ws/src
git clone --recursive https://github.com/leggedrobotics/darknet_ros.git
#因为这样克隆的darknet文件夹是空的，将darknet_ros中的darknet的文件替换成如下
cd darknet_ros
git clone https://github.com/arnoldfychen/darknet.git
#进入darknet目录下进行make
cd darknet
make

修改darknet文件夹下的Makefile文件

修改 darknet_ros/darknet_ros/CMakeLists.txt

将这算力改成自己电脑的算力

make可能会报如下错误：

一、

fatal error:cudnn_ros_infer.h:没有那个文件或目录

解决办法：

根据官方链接cuDNN Archive | NVIDIA Developer，选择与cuda版本相匹配的cudnn下载tar文件。在这里，我下载的是v8.9.6，可与cuda 12.x等版本适配。

解压cudnn-linux-x86_64-8.9.7.29_cuda11-archive.tar.xz后，进入该目录，将lib内的文件都复制到目录/usr/local/cuda-11.6/lib64/中，将include内的文件都复制到目录/usr/local/cuda-11.6/include/中

cd ./cudnn-linux-x86_64-8.9.6.50_cuda11-archive
sudo cp ./lib/* /usr/local/cuda-12.2/lib64/
sudo cp ./include/* /usr/local/cuda-12.2/include/

二、

fatal error:opencv2/opencvhpp:没有那个文件或目录

解决办法：修改修改darknet文件夹下的Makefile文件

三、

error: ‘IplImage’ does not name a type 12 | IplImage *image_to_ipl(image im)

把 home/darknet/src/imageopencv.cpp 修改如下：

#ifdef OPENCV

#include "stdio.h"
#include "stdlib.h"
#include "opencv2/opencv.hpp"
#include "image.h"

using namespace cv;

extern "C" {

/*IplImage *image_to_ipl(image im)
{
    int x,y,c;
    IplImage *disp = cvCreateImage(cvSize(im.w,im.h), IPL_DEPTH_8U, im.c);
    int step = disp->widthStep;
    for(y = 0; y < im.h; ++y){
        for(x = 0; x < im.w; ++x){
            for(c= 0; c < im.c; ++c){
                float val = im.data[c*im.h*im.w + y*im.w + x];
                disp->imageData[y*step + x*im.c + c] = (unsigned char)(val*255);
            }
        }
    }
    return disp;
}

image ipl_to_image(IplImage* src)
{
    int h = src->height;
    int w = src->width;
    int c = src->nChannels;
    image im = make_image(w, h, c);
    unsigned char *data = (unsigned char *)src->imageData;
    int step = src->widthStep;
    int i, j, k;

    for(i = 0; i < h; ++i){
        for(k= 0; k < c; ++k){
            for(j = 0; j < w; ++j){
                im.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
            }
        }
    }
    return im;
}*/

/*Mat image_to_mat(image im)
{
    image copy = copy_image(im);
    constrain_image(copy);
    if(im.c == 3) rgbgr_image(copy);

    IplImage *ipl = image_to_ipl(copy);
    Mat m = cvarrToMat(ipl, true);
    cvReleaseImage(&ipl);
    free_image(copy);
    return m;
}

image mat_to_image(Mat m)
{
    IplImage ipl = m;
    image im = ipl_to_image(&ipl);
    rgbgr_image(im);
    return im;
}*/

Mat image_to_mat(image im)
{
    image copy = copy_image(im);
    constrain_image(copy);
    if(im.c == 3) rgbgr_image(copy);

    Mat m(cv::Size(im.w,im.h), CV_8UC(im.c));
    int x,y,c;

    int step = m.step;
    for(y = 0; y < im.h; ++y){
        for(x = 0; x < im.w; ++x){
            for(c= 0; c < im.c; ++c){
                float val = im.data[c*im.h*im.w + y*im.w + x];
                m.data[y*step + x*im.c + c] = (unsigned char)(val*255);
            }
        }
    }

    free_image(copy);
    return m;

// free_image(copy);
// return m;
//     IplImage *ipl = image_to_ipl(copy);
//     Mat m = cvarrToMat(ipl, true);
//     cvReleaseImage(&ipl);
//     free_image(copy);
//     return m;
}

image mat_to_image(Mat m)
{
    int h = m.rows;
    int w = m.cols;
    int c = m.channels();
    image im = make_image(w, h, c);
    unsigned char *data = (unsigned char *)m.data;
    int step = m.step;
    int i, j, k;

    for(i = 0; i < h; ++i){
        for(k= 0; k < c; ++k){
            for(j = 0; j < w; ++j){
                im.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
            }
        }
    }
    rgbgr_image(im);
    return im;
    // IplImage ipl = m;
    // image im = ipl_to_image(&ipl);
    // rgbgr_image(im);
    // return im;
}

void *open_video_stream(const char *f, int c, int w, int h, int fps)
{
    VideoCapture *cap;
    if(f) cap = new VideoCapture(f);
    else cap = new VideoCapture(c);
    if(!cap->isOpened()) return 0;
    //if(w) cap->set(CV_CAP_PROP_FRAME_WIDTH, w);
    //if(h) cap->set(CV_CAP_PROP_FRAME_HEIGHT, w);
    //if(fps) cap->set(CV_CAP_PROP_FPS, w);
    if(w) cap->set(CAP_PROP_FRAME_WIDTH, w);
    if(h) cap->set(CAP_PROP_FRAME_HEIGHT, w);
    if(fps) cap->set(CAP_PROP_FPS, w);
    return (void *) cap;
}

image get_image_from_stream(void *p)
{
    VideoCapture *cap = (VideoCapture *)p;
    Mat m;
    *cap >> m;
    if(m.empty()) return make_empty_image(0,0,0);
    return mat_to_image(m);
}

image load_image_cv(char *filename, int channels)
{
    int flag = -1;
    if (channels == 0) flag = -1;
    else if (channels == 1) flag = 0;
    else if (channels == 3) flag = 1;
    else {
        fprintf(stderr, "OpenCV can't force load with %d channels\n", channels);
    }
    Mat m;
    m = imread(filename, flag);
    if(!m.data){
        fprintf(stderr, "Cannot load image \"%s\"\n", filename);
        char buff[256];
        sprintf(buff, "echo %s >> bad.list", filename);
        system(buff);
        return make_image(10,10,3);
        //exit(0);
    }
    image im = mat_to_image(m);
    return im;
}

int show_image_cv(image im, const char* name, int ms)
{
    Mat m = image_to_mat(im);
    imshow(name, m);
    int c = waitKey(ms);
    if (c != -1) c = c%256;
    return c;
}

void make_window(char *name, int w, int h, int fullscreen)
{
    namedWindow(name, WINDOW_NORMAL); 
    if (fullscreen) {
        //setWindowProperty(name, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
        setWindowProperty(name, WND_PROP_FULLSCREEN, WINDOW_FULLSCREEN);
    } else {
        resizeWindow(name, w, h);
        if(strcmp(name, "Demo") == 0) moveWindow(name, 0, 0);
    }
}

}

#endif

将之前在darknet文件进行make出错的文件删除，再重新编译

sudo make clean
make

在工作空间中安装realsense_ros与机械臂启动包

cd yolo_ws
cd src
#安装realsense_ros
git clone https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git fmauch_universal_robot

cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release

使用VScode 在工作空间的src目录下创建新的文件夹std_msg，并添加相关的依赖：rospy roscpp std_msgs

在std_msg文件中创建scripts文件夹，创建两个py文件

一个为yolo目标检测文件及机械臂运动程序

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from darknet_ros_msgs.msg import BoundingBoxes, BoundingBox
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
import numpy as np
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import Point


class Readyolo:
    def __init__(self):
        rospy.init_node('grasping_node', anonymous=True)

        self.bridge = CvBridge()

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        self.rgb_image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.rgb_image_callback)
        self.depth_image_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.depth_image_callback)
        self.camera_info_sub = rospy.Subscriber('/camera/depth/camera_info', CameraInfo, self.camera_info_callback)

        self.object_detection = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.yolo)
        self.pub = rospy.Publisher('/object_camera_coordinates', Point, queue_size=10)

        self.depth_intrinsics = None
        self.rgb_image = None
        self.depth_image = None

    def camera_info_callback(self, msg):
        self.depth_intrinsics = msg
        print(self.depth_intrinsics.K[0])
        print(self.depth_intrinsics.K[4])
        print()

    def rgb_image_callback(self, msg):
        self.rgb_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")

    def depth_image_callback(self, msg):
        self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
        self.process_yolo_results()

    def process_yolo_results(self):
        if self.depth_intrinsics is None or self.rgb_image is None or self.depth_image is None:
            return

        for box in self.bounding_boxes:
            if box.Class == "bottle":
                yolo_center_point_x = (box.xmin + box.xmax) / 2
                yolo_center_point_y = (box.ymin + box.ymax) / 2

                depth = self.depth_image[int(yolo_center_point_y)][int(yolo_center_point_x)]

                camera_point = self.convert_pixel_to_camera_coordinates(yolo_center_point_x, yolo_center_point_y, depth)

                print("OI:",yolo_center_point_x,yolo_center_point_y)
                point_msg = Point(x=camera_point[0], y=camera_point[1], z=camera_point[2])
                self.pub.publish(point_msg)


                # self.pub.publish(box)

    def convert_pixel_to_camera_coordinates(self, u, v, depth):
        if self.depth_intrinsics is None:
            return None

        fx = self.depth_intrinsics.K[0]
        fy = self.depth_intrinsics.K[4]
        cx = self.depth_intrinsics.K[2]
        cy = self.depth_intrinsics.K[5]

        camera_x = (u - cx) * depth / fx/1000
        camera_y = (v - cy) * depth / fy/1000
        camera_z = float(depth)/1000
        print("camera_link:",camera_x,camera_y,camera_z)

        return [camera_x, camera_y, camera_z]

    def yolo(self, msg):
        self.bounding_boxes = msg.bounding_boxes
        self.process_yolo_results()

def main():
    readyolo = Readyolo()
    rospy.spin()

if __name__ == '__main__':
    main()

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose
import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import Point

class MoveItIkDemo:

    def __init__(self):

        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_it_demo')

        # 初始化需要使用move group控制的机械臂中的arm group
        self.arm = moveit_commander.MoveGroupCommander('manipulator')

        # 获取终端link的名称,这个在setup assistant中设置过了
        end_effector_link = self.arm.get_end_effector_link()

        # 设置目标位置所使用的参考坐标系
        reference_frame = "base"
        self.arm.set_pose_reference_frame(reference_frame)

        # 当运动规划失败后,允许重新规划
        self.arm.allow_replanning(True)

        # 设置位置(单位: 米)和姿态(单位:弧度)的允许误差
        # self.arm.set_goal_position_tolerance(0.001)
        # self.arm.set_goal_orientation_tolerance(0.01)

        self.arm.set_goal_position_tolerance(0.001000)
        self.arm.set_goal_orientation_tolerance(0.01000)

        # 设置允许的最大速度和加速度
        self.arm.set_max_acceleration_scaling_factor(0.5)
        self.arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始位置
        # self.arm.set_named_target('home')
        # self.arm.go(wait=True)
        # rospy.sleep(1)

        # 设置机械臂工作空间中的目标位姿,位置使用x y z坐标描述
        # 姿态使用四元数描述,基于base_link坐标系
        # target_pose = PoseStamped()
        # # 参考坐标系,前面设置了
        # target_pose.header.frame_id = reference_frame
        # target_pose.header.stamp = rospy.Time.now()
        # # 末端位置
        # target_pose.pose.position.x = 0.359300
        # target_pose.pose.position.y = 0.163600
        # target_pose.pose.position.z = 0.278700
        # # 末端姿态,四元数
        # target_pose.pose.orientation.x = 0.433680
        # target_pose.pose.orientation.y = 0.651417
        # target_pose.pose.orientation.z = 0.508190
        # target_pose.pose.orientation.w = 0.359611

        # # 设置机械臂当前的状态作为运动初始状态
        # self.arm.set_start_state_to_current_state()
        # # 设置机械臂终端运动的目标位姿
        # self.arm.set_pose_target(target_pose)
        # # 规划运动路径,返回虚影的效果
        # plan_success,traj,planning_time,error_code = self.arm.plan()
        # # traj = self.arm.plan()

        # # 按照规划的运动路径控制机械臂运动
        # self.arm.execute(traj)
        # # 执行完休息一秒
        # rospy.sleep(1)

        # 新建一个接收方用来接受话题并处理
        self.pose_sub = rospy.Subscriber("/object_camera_coordinates", Point, self.callback) 
    

    def transform_pose(self, input_pose, from_frame, to_frame):
        try:
            tf_buffer = tf2_ros.Buffer()
            listener = tf2_ros.TransformListener(tf_buffer)
            transform = tf_buffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
            transformed_pose = tf2_geometry_msgs.do_transform_pose(input_pose, transform)
            return transformed_pose
        except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationoException) as ex:
            rospy.logerr("TF2 error:%s",str(ex))
            return None
        

    def callback(self, p):
        global object_position, grasp_pose
        object_position = p
        temp_pose = PoseStamped()
        temp_pose.header.frame_id = "camera_link"
        temp_pose.pose.position = p
        temp_pose.pose.orientation.w = 1.0
        transformed_pose = self.transform_pose(temp_pose, "camera_link", "base")
        if transformed_pose is not None:
            grasp_pose = transformed_pose
            self.arm.set_start_state_to_current_state()
            self.arm.set_pose_target(grasp_pose)
            plan_success,traj,planning_time,error_code = self.arm.plan()
            self.arm.execute(traj)
            rospy.sleep(1)


def main():
    moveitikdemo = MoveItIkDemo()
    rospy.spin()


if __name__ == '__main__':
    main()

修改py文件权限，并在CMakeLists.txt进行相对应的修改