From b8ee6672d18c49474285af4150bceffe0f2359db Mon Sep 17 00:00:00 2001 From: admin1 Date: Wed, 17 Aug 2022 15:43:07 +0300 Subject: [PATCH] fixed cubes rendering. created additional small node for publishing the cubes. debug process made easier --- ov_msckf/cmake/ROS1.cmake | 9 +- ov_msckf/launch/subscribe.launch | 1 + ov_msckf/src/render_cubes.cpp | 87 +++++++++++++++++++ ov_msckf/src/ros/ROS1Visualizer.cpp | 128 +++++++++++++++------------- 4 files changed, 166 insertions(+), 59 deletions(-) create mode 100644 ov_msckf/src/render_cubes.cpp diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 7109877..a2c9f00 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -131,7 +131,14 @@ if (catkin_FOUND AND ENABLE_ROS) install(TARGETS image_render_combine ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION $[CATKIN_PACKAGE_BIN_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + + add_executable(render_cubes src/render_cubes.cpp) + target_link_libraries(render_cubes ov_msckf_lib ${thirdparty_libraries}) + install(TARGETS render_cubes + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) endif () diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 751978a..cbfef6c 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -29,6 +29,7 @@ + diff --git a/ov_msckf/src/render_cubes.cpp b/ov_msckf/src/render_cubes.cpp new file mode 100644 index 0000000..9386577 --- /dev/null +++ b/ov_msckf/src/render_cubes.cpp @@ -0,0 +1,87 @@ +// +// Created by Podmogilnyi Ivan on 17.08.2022. +// +#include +#include +#include +#include + +float yCubesVisPos = -2.0; +float skew = 5.0; +std::vector> cubeVisPoints = { + {71.215, 32.694, yCubesVisPos+2.5}, +// {81.668, 26.227, yCubesVisPos}, + {104.061, 36.691, yCubesVisPos+2.0}, +// {90.726, 32.825, yCubesVisPos}, + {109.541, 60.415, yCubesVisPos+1.5}, +// {149.264, 69.490, yCubesVisPos}, + {183.605, 91.474, yCubesVisPos+0.5}, +// {198.294, 97.113, yCubesVisPos}, + {251.391, 154.108, yCubesVisPos-1.0}, +// {310.030, 190.208, yCubesVisPos}, + {330.554, 183.244, yCubesVisPos-2.0}, + {746.086, 450.563, yCubesVisPos-2.0}, +// {792.608, 527.140, yCubesVisPos}, + {883.669, 517.444, yCubesVisPos-2.0}, + {957.811, 652.638, yCubesVisPos-2.0}, + {1086.045, 669.317, yCubesVisPos-2.0}, + {1125.310, 728.214, yCubesVisPos-2.0}, + {731.996, 425.021, yCubesVisPos-2.0}, + + {1186.527, 700.189, yCubesVisPos}, +// {1205.432, 681.457, yCubesVisPos}, + {1184.375, 735.604, yCubesVisPos}, +// {1232.697, 715.647, yCubesVisPos}, + {1244.767, 784.257, yCubesVisPos} +}; + +ros::Publisher pub_cubes; +ros::Subscriber sub_imu; +visualization_msgs::Marker Marker; + +void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){ + ROS_INFO_STREAM("received imu"); + pub_cubes.publish(Marker); + +} + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "render_cubes"); + + ros::NodeHandle nh; + + // the idea: set the subscriber for IMU, and every time the IMU measurement comes, publish + // the cubes because the images are also publish when the IMU measurement arrives. + sub_imu = nh.subscribe("/imu/data_raw", 1000, &callback_imu); + pub_cubes = nh.advertise("/ov_msckf/vis_markers", 0); + + Marker.header.frame_id = "global"; + Marker.header.stamp = ros::Time(); + Marker.ns = "my_namespace"; + Marker.id = 0; + Marker.type = visualization_msgs::Marker::CUBE_LIST; + Marker.action = visualization_msgs::Marker::ADD; + Marker.pose.orientation.x = 0.0; + Marker.pose.orientation.y = 0.0; + Marker.pose.orientation.z = 0.0; + Marker.pose.orientation.w = 1.0; + Marker.scale.x = 6.8f; + Marker.scale.y = 6.8f; + Marker.scale.z = 6.8f; + Marker.color.a = 1.0; // Don't forget to set the alpha! + Marker.color.r = 0.0; + Marker.color.g = 1.0; + Marker.color.b = 0.0; +//only if using a MESH_RESOURCE marker type: + Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; + + for (auto & cubeVisPoint : cubeVisPoints){ + geometry_msgs::Point tempPoint; + tempPoint.x = cubeVisPoint[0]; + tempPoint.y = cubeVisPoint[1]; + tempPoint.z = cubeVisPoint[2]; + Marker.points.push_back(tempPoint); + } + + ros::spin(); +} diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 09b094d..d1258d2 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -93,7 +93,8 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ } fT_aligned << "ts mT_aligned" << std::endl; - subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this); + //// UNCOMMENT THIS TO RESTORE THE T_align_callback +// subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this); std::ifstream fileTraj; fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt"); @@ -135,30 +136,37 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ pub_camera_info = nh->advertise("/ov_msckf/camera_info", 2); pub_cam0_info = nh->advertise("/ov_msckf/cam0_info", 2); - vis_pub = nh->advertise("/ov_msckf/vis_markers", 0); +//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING +// vis_pub = nh->advertise("/ov_msckf/vis_markers", 0); // pub_camera_info = nh->advertise("/ov_mskcf/camera_info", 2); // pub_camera_info_trackhist = nh->advertise("/ov_msckf/trackhist", 2); // camPub = nh->advertise("ov_msckf/camera_info", 2); //camPub = it.advertiseCamera("/ov_msckf/image_view", 2); - Marker.header.frame_id = "global"; - Marker.header.stamp = ros::Time(); - Marker.ns = "my_namespace"; - Marker.id = 0; - Marker.type = visualization_msgs::Marker::CUBE_LIST; - Marker.action = visualization_msgs::Marker::ADD; - Marker.pose.orientation.x = 0.0; - Marker.pose.orientation.y = 0.0; - Marker.pose.orientation.z = 0.0; - Marker.pose.orientation.w = 1.0; - Marker.scale.x = 6.8f; - Marker.scale.y = 6.8f; - Marker.scale.z = 6.8f; - Marker.color.a = 1.0; // Don't forget to set the alpha! - Marker.color.r = 0.0; - Marker.color.g = 1.0; - Marker.color.b = 0.0; - //only if using a MESH_RESOURCE marker type: - Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; + +//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING +// +// Marker.header.frame_id = "global"; +// Marker.header.stamp = ros::Time(); +// Marker.ns = "my_namespace"; +// Marker.id = 0; +// Marker.type = visualization_msgs::Marker::CUBE_LIST; +// Marker.action = visualization_msgs::Marker::ADD; +// Marker.pose.orientation.x = 0.0; +// Marker.pose.orientation.y = 0.0; +// Marker.pose.orientation.z = 0.0; +// Marker.pose.orientation.w = 1.0; +// Marker.scale.x = 6.8f; +// Marker.scale.y = 6.8f; +// Marker.scale.z = 6.8f; +// Marker.color.a = 1.0; // Don't forget to set the alpha! +// Marker.color.r = 0.0; +// Marker.color.g = 1.0; +// Marker.color.b = 0.0; +// //only if using a MESH_RESOURCE marker type: +// Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; + + + // cubeVisPoints = {{10.0, 10.0, 0.02}, {10, -10, 0.02}, // {50, 10, 0.02}, {50, 30, 0.02}, @@ -168,36 +176,39 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // {240, 90, 0.02}, {80, 110, 0.02}, // {280, 110, 0.02}, {100, 130, 0.02}, // {320, 130, 0.02}, {320, 150, 0.02}}; - float yCubesVisPos = -6.0; - float skew = 5.0; - cubeVisPoints = { - {71.215, 32.694, yCubesVisPos+3}, -// {81.668, 26.227, yCubesVisPos}, - {84.455, 41.731, yCubesVisPos+3}, -// {90.726, 32.825, yCubesVisPos}, - {109.541, 60.415, yCubesVisPos+3}, -// {149.264, 69.490, yCubesVisPos}, - {150.645, 89.474, yCubesVisPos+2}, -// {198.294, 97.113, yCubesVisPos}, - {251.391, 154.108, yCubesVisPos}, -// {310.030, 190.208, yCubesVisPos}, - {330.554, 183.244, yCubesVisPos}, - {746.086, 450.563, yCubesVisPos}, -// {792.608, 527.140, yCubesVisPos}, - {883.669, 517.444, yCubesVisPos}, - {957.811, 652.638, yCubesVisPos}, - {1086.045, 669.317, yCubesVisPos}, - {1125.310, 728.214, yCubesVisPos}, - {731.996, 425.021, yCubesVisPos}, - {1186.527, 700.189, yCubesVisPos}, -// {1205.432, 681.457, yCubesVisPos}, - {1184.375, 735.604, yCubesVisPos}, -// {1232.697, 715.647, yCubesVisPos}, - {1244.767, 784.257, yCubesVisPos} - }; - cashedCubeVisPoints = cubeVisPoints; - // End of added by PI + +//// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING +// float yCubesVisPos = 3.0; +// float skew = 5.0; +// cubeVisPoints = { +// {71.215, 32.694, yCubesVisPos+3}, +//// {81.668, 26.227, yCubesVisPos}, +// {84.455, 41.731, yCubesVisPos+3}, +//// {90.726, 32.825, yCubesVisPos}, +// {109.541, 60.415, yCubesVisPos+3}, +//// {149.264, 69.490, yCubesVisPos}, +// {150.645, 89.474, yCubesVisPos+2}, +//// {198.294, 97.113, yCubesVisPos}, +// {251.391, 154.108, yCubesVisPos}, +//// {310.030, 190.208, yCubesVisPos}, +// {330.554, 183.244, yCubesVisPos}, +// {746.086, 450.563, yCubesVisPos}, +//// {792.608, 527.140, yCubesVisPos}, +// {883.669, 517.444, yCubesVisPos}, +// {957.811, 652.638, yCubesVisPos}, +// {1086.045, 669.317, yCubesVisPos}, +// {1125.310, 728.214, yCubesVisPos}, +// {731.996, 425.021, yCubesVisPos}, +// +// {1186.527, 700.189, yCubesVisPos}, +//// {1205.432, 681.457, yCubesVisPos}, +// {1184.375, 735.604, yCubesVisPos}, +//// {1232.697, 715.647, yCubesVisPos}, +// {1244.767, 784.257, yCubesVisPos} +// }; +// cashedCubeVisPoints = cubeVisPoints; +// // End of added by PI // option `to enable publishing of global to IMU transformation nh->param("publish_global_to_imu_tf", publish_global2imu_tf, true); @@ -772,14 +783,15 @@ void ROS1Visualizer::publish_images() { camInfoGlob.width = exl_msg->width / 2; pub_cam0_info.publish(camInfoGlob); - for (auto & cubeVisPoint : cubeVisPoints){ - geometry_msgs::Point tempPoint; - tempPoint.x = cubeVisPoint[0]; - tempPoint.y = cubeVisPoint[1]; - tempPoint.z = cubeVisPoint[2]; - Marker.points.push_back(tempPoint); - } - vis_pub.publish( Marker ); +//// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING +// for (auto & cubeVisPoint : cubeVisPoints){ +// geometry_msgs::Point tempPoint; +// tempPoint.x = cubeVisPoint[0]; +// tempPoint.y = cubeVisPoint[1]; +// tempPoint.z = cubeVisPoint[2]; +// Marker.points.push_back(tempPoint); +// } +// vis_pub.publish( Marker ); // pub_camera_info.publish(cameraparams); // pub_camera_info_trackhist.publish(cameraparams);