diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index 60da200..2b902b8 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -347,10 +347,10 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons double y1 = row_1; double x2 = col_2; double y2 = row_2; - x1 = x1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x2 - x1); - y1 = y1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y2 - y1); - x2 = x2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x1 - x2); - y2 = y2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y1 - y2); + // x1 = x1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x2 - x1); + // y1 = y1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y2 - y1); + // x2 = x2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x1 - x2); + // y2 = y2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y1 - y2); cv::line(projected_edges, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(255, 255, 255), dlo_pixel_width); } diff --git a/trackdlo/src/utils.cpp b/trackdlo/src/utils.cpp index ae03ec6..aa8f22f 100644 --- a/trackdlo/src/utils.cpp +++ b/trackdlo/src/utils.cpp @@ -263,7 +263,7 @@ visualization_msgs::MarkerArray MatrixXd2MarkerArray (MatrixXd Y, // cur_node_result.header.stamp = ros::Time::now(); cur_node_result.type = visualization_msgs::Marker::SPHERE; cur_node_result.action = visualization_msgs::Marker::ADD; - cur_node_result.ns = marker_ns + std::to_string(i); + cur_node_result.ns = marker_ns + "_node_" + std::to_string(i); cur_node_result.id = i; // add position @@ -309,10 +309,10 @@ visualization_msgs::MarkerArray MatrixXd2MarkerArray (MatrixXd Y, visualization_msgs::Marker cur_line_result = visualization_msgs::Marker(); // add header - cur_line_result.header.frame_id = "camera_color_optical_frame"; + cur_line_result.header.frame_id = marker_frame; cur_line_result.type = visualization_msgs::Marker::CYLINDER; cur_line_result.action = visualization_msgs::Marker::ADD; - cur_line_result.ns = "line_results" + std::to_string(i); + cur_line_result.ns = marker_ns + "_line_" + std::to_string(i); cur_line_result.id = i; // add position @@ -381,7 +381,7 @@ visualization_msgs::MarkerArray MatrixXd2MarkerArray (std::vector Y, // cur_node_result.header.stamp = ros::Time::now(); cur_node_result.type = visualization_msgs::Marker::SPHERE; cur_node_result.action = visualization_msgs::Marker::ADD; - cur_node_result.ns = marker_ns + std::to_string(i); + cur_node_result.ns = marker_ns + "_node_" + std::to_string(i); cur_node_result.id = i; // add position @@ -427,10 +427,10 @@ visualization_msgs::MarkerArray MatrixXd2MarkerArray (std::vector Y, visualization_msgs::Marker cur_line_result = visualization_msgs::Marker(); // add header - cur_line_result.header.frame_id = "camera_color_optical_frame"; + cur_line_result.header.frame_id = marker_frame; cur_line_result.type = visualization_msgs::Marker::CYLINDER; cur_line_result.action = visualization_msgs::Marker::ADD; - cur_line_result.ns = "line_results" + std::to_string(i); + cur_line_result.ns = marker_ns + "_line_" + std::to_string(i); cur_line_result.id = i; // add position