the drone_detect package seems not work

[huang-shijie]

Hi @bigsuperZZZX i am using these project in gazebo. but when i use the drone_detect package ,the new_depth topic can not erase hit pixels in depth. i config file are follows. I configured the code according to the readme.

two config file cam_width: 640 cam_height: 480 cam_fx: 554.254691191187 cam_fy: 554.254691191187 cam_cx: 320.5 cam_cy: 240.5

pixel_ratio: 0.1 estimate/max_pose_error: 0.4 estimate/drone_width: 0.5 estimate/drone_height: 0.2

source file ` DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle) : nh_(nodeHandle) { readParameters();

// depth_imgsub.reset(new message_filters::Subscriber(nh_, "depth", 50, ros::TransportHints().tcpNoDelay())); // colordepth_imgsub.reset(new message_filters::Subscriber(nh_, "colordepth", 50)); // camera_possub.reset(new message_filters::Subscriber(nh_, "camera_pose", 50));

my_odomsub = nh_.subscribe("odometry", 100, &DroneDetector::rcvMyOdomCallback, this, ros::TransportHints().tcpNoDelay()); depth_imgsub = nh_.subscribe("depth", 50, &DroneDetector::rcvDepthImgCallback, this, ros::TransportHints().tcpNoDelay()); // sync_depth_color_imgpose->registerCallback(boost::bind(&DroneDetector::rcvDepthColorCamPoseCallback, this, _1, _2, _3));

drone0_odomsub = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this); drone1_odomsub = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this); drone2_odomsub = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this); drone3_odomsub = nh_.subscribe("drone3", 50, &DroneDetector::rcvDrone3OdomCallback, this); // droneX_odomsub = nh_.subscribe("others_odom", 50, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay());

new_depth_imgpub = nh_.advertise("new_depth_image", 50); debug_depth_imgpub = nh_.advertise("debug_depth_image", 50);

debug_infopub = nh_.advertise("/debug_info", 50);

cam2body << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; // cam2body << // 1.0, 0.0, 1.0, -0.25, // 0.0, 1.0, 0.0, 0.0, // 0.0, 0.0, 1.0, 0.0, // 0.0, 0.0, 0.0, 1.0;

// init drone_pose_err_pub for(int i = 0; i < max_dronenum; i++) { if(i != myid) drone_pose_errpub[i] = nh_.advertise("drone"+std::to_string(i)+"to"+std::to_string(myid)+"_pose_err", 50); }

ROS_INFO("Successfully launched node."); } `

launch file `

`

[huang-shijie]

i find this question is caused by the .toimagemsg() function not work. it seems is due to the encoding stratage. the depth image encoding is 32FC1. dose the cv bridge support this encoding stratage? the imshow() seems ok,but the ros publish image is false , and i use the ubuntu 20.04 ,ros noetic.

`void DroneDetector::rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img) { / get depth image / cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(depth_img, depth_img->encoding); cv_ptr->image.copyTo(depthimg);

// cv::Mat depthimg = cv_bridge::toCvShare(depth_img)->image;

debug_starttime = ros::Time::now();

Eigen::Vector2i true_pixel[max_dronenum]; //数组 for (int i = 0; i < max_dronenum; i++) { if (indepth[i]) { detect(i, true_pixel[i]); } }

cv_bridge::CvImage out_msg; for (int i = 0; i < max_dronenum; i++) { if (indepth[i]) { // erase hit pixels in depth for(int k = 0; k < int(hitpixels[i].size()); k++) { // depthimg.at(hitpixels[i]k, hitpixels[i]k) = 0; uint16_t row_ptr; row_ptr = depthimg.ptr(hitpixels[i]k); ((row_ptr+hitpixels[i]k)) = 0.0; // std::cout << "remove _" <<std::endl; } } } cv::imshow("Image Viewer", depthimg); cv::waitKey(10); // depthimg debug_endtime = ros::Time::now(); // ROS_WARN("cost_total_time = %lf", (debug_endtime - debug_starttime).toSec()*1000.0); out_msg.header = depth_img->header; out_msg.encoding = depthimg->encodingsss; // std::cout << "remove "<< out_msg.encoding <<std::endl; out_msg.image = depthimg.clone();

// sensor_msgs::ImagePtr modified_depth_msg = cv_bridge::CvImage(depth_img->header, "32FC1", depthimg).toImageMsg();

new_depth_imgpub.publish(out_msg.toImageMsg());

std_msgs::String msg; std::stringstream ss; if(debugflag) { for (int i = 0; i < max_dronenum; i++) { if (indepth[i]) { // add bound box in colormap // cv::Rect rect(_bbox_lu.x, _bbox_lu.y, _bbox_rd.x, _bbox_rd.y);//左上坐标（x,y）和矩形的长(x)宽(y) cv::rectangle(depthimg, cv::Rect(searchboxlu[i], searchboxrd[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); cv::rectangle(depthimg, cv::Rect(boundingboxlu[i], boundingboxrd[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0); if (debug_detectresult[i] == 1) { ss << "no enough " << hitpixels[i].size(); } else if(debug_detectresult[i] == 2) { ss << "success"; } } else { ss << "no detect"; } } out_msg.header = depth_img->header; out_msg.encoding = depth_img->encoding; out_msg.image = depthimg.clone(); debug_depth_imgpub.publish(out_msg.toImageMsg()); msg.data = ss.str(); debug_infopub.publish(msg); } }`

[bigsuperZZZX]

This part of code may help: https://github.com/ZJU-FAST-Lab/EGO-Planner-v2/blob/9d85475ea7b9bf5c112cf7c3c3d0d3f9e96d9010/swarm-playground/main_ws/src/planner/plan_env/src/grid_map.cpp#L239