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_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "depth", 50, ros::TransportHints().tcpNoDelay()));
// colordepth_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "colordepth", 50));
// camera_pos_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(nh_, "camera_pose", 50));

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

drone0_odom_sub_ = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this);
drone1_odom_sub_ = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this);
drone2_odom_sub_ = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this);
drone3_odom_sub_ = nh_.subscribe("drone3", 50, &DroneDetector::rcvDrone3OdomCallback, this);
// droneX_odom_sub_ = nh_.subscribe("others_odom", 50, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay());

new_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("new_depth_image", 50);
debug_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("debug_depth_image", 50);

debug_info_pub_ = nh_.advertise<std_msgs::String>("/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_drone_num_; i++) {
if(i != my_id_)
drone_pose_err_pub_[i] = nh_.advertise<geometry_msgs::PoseStamped>("drone"+std::to_string(i)+"to"+std::to_string(my_id_)+"_pose_err", 50);
}

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

launch file
`

    <remap from="~odometry"   to="iris_$(arg drone_id)/$(arg odom_topic)"/>
    <remap from="~depth" to="iris_$(arg drone_id)/realsense/depth_camera/depth/image_raw"/>
    <remap from="~new_depth_image" to="iris_$(arg drone_id)/new_depth_image"/>
    <remap from="~debug_depth_image" to="iris_$(arg drone_id)/debug_depth_image"/>
    <!-- <remap from="~colordepth" to="iris_$(arg drone_id)/realsense/depth_camera/color/image_raw"/>
    <remap from="~camera_pose" to="null"/> -->

    <remap from="~drone0" to="/iris_0/$(arg odom_topic)"/>
    <remap from="~drone1" to="/iris_1/$(arg odom_topic)"/>
    <remap from="~drone2" to="/iris_2/$(arg odom_topic)"/>
    <remap from="~drone3" to="/iris_3/$(arg odom_topic)"/>

`

[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(depth_img_);

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

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

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

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

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

new_depth_img_pub_.publish(out_msg.toImageMsg());

std_msgs::String msg;
std::stringstream ss;
if(debug_flag_) {
for (int i = 0; i < max_drone_num_; i++) {
if (in_depth_[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(depth_img, cv::Rect(searchbox_lu[i], searchbox_rd[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
cv::rectangle(depth_img, cv::Rect(boundingbox_lu_[i], boundingbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
if (debug_detect_result_[i] == 1) {
ss << "no enough " << hit_pixels_[i].size();
} else if(debug_detect_result_[i] == 2) {
ss << "success";
}
} else {
ss << "no detect";
}
}
out_msg.header = depth_img->header;
out_msg.encoding = depth_img->encoding;
out_msg.image = depth_img_.clone();
debug_depth_img_pub_.publish(out_msg.toImageMsg());
msg.data = ss.str();
debug_info_pub_.publish(msg);
}
}`

[bigsuperZZZX]

This part of code may help:

EGO-Planner-v2/swarm-playground/main_ws/src/planner/plan_env/src/grid_map.cpp

Line 239 in 9d85475

if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)