pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fixed cubes rendering. created additional small node for publishing the cubes. debug process made easier

Browse Source
This commit is contained in:
admin1
2022-08-17 15:43:07 +03:00
parent f8843bf0fc
commit b8ee6672d1
4 changed files with 166 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
ov_msckf/cmake/ROS1.cmake
View File

@@ -131,7 +131,14 @@ if (catkin_FOUND AND ENABLE_ROS)
install(TARGETS image_render_combine install(TARGETS image_render_combine
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY 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 () endif ()

1
ov_msckf/launch/subscribe.launch
View File

@@ -29,6 +29,7 @@
<!-- MASTER NODE! --> <!-- MASTER NODE! -->
<!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">--> <!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
<node name="image_render_combine" pkg="ov_msckf" type="image_render_combine" output="screen" clear_params="true" required="true"> </node> <node name="image_render_combine" pkg="ov_msckf" type="image_render_combine" output="screen" clear_params="true" required="true"> </node>
<node name="render_cubes" pkg="ov_msckf" type="render_cubes" output="screen" clear_params="true" required="true"></node>
<node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true"> <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
<!-- master configuration object --> <!-- master configuration object -->

87
ov_msckf/src/render_cubes.cpp Normal file
View File

@@ -0,0 +1,87 @@
//
// Created by Podmogilnyi Ivan on 17.08.2022.
//
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/CameraInfo.h>
float yCubesVisPos = -2.0;
float skew = 5.0;
std::vector<std::vector<double>> 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<visualization_msgs::Marker>("/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();
}

128
ov_msckf/src/ros/ROS1Visualizer.cpp
View File

@@ -93,7 +93,8 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
} }
fT_aligned << "ts mT_aligned" << std::endl; 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; std::ifstream fileTraj;
fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt"); fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt");
@@ -135,30 +136,37 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", 2); pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", 2);
pub_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/cam0_info", 2); pub_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/cam0_info", 2);
vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0); //// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
// vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
// pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2); // pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2);
// pub_camera_info_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2); // pub_camera_info_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2);
// camPub = nh->advertise<image_transport::CameraPublisher>("ov_msckf/camera_info", 2); // camPub = nh->advertise<image_transport::CameraPublisher>("ov_msckf/camera_info", 2);
//camPub = it.advertiseCamera("/ov_msckf/image_view", 2); //camPub = it.advertiseCamera("/ov_msckf/image_view", 2);
Marker.header.frame_id = "global";
Marker.header.stamp = ros::Time(); //// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
Marker.ns = "my_namespace"; //
Marker.id = 0; // Marker.header.frame_id = "global";
Marker.type = visualization_msgs::Marker::CUBE_LIST; // Marker.header.stamp = ros::Time();
Marker.action = visualization_msgs::Marker::ADD; // Marker.ns = "my_namespace";
Marker.pose.orientation.x = 0.0; // Marker.id = 0;
Marker.pose.orientation.y = 0.0; // Marker.type = visualization_msgs::Marker::CUBE_LIST;
Marker.pose.orientation.z = 0.0; // Marker.action = visualization_msgs::Marker::ADD;
Marker.pose.orientation.w = 1.0; // Marker.pose.orientation.x = 0.0;
Marker.scale.x = 6.8f; // Marker.pose.orientation.y = 0.0;
Marker.scale.y = 6.8f; // Marker.pose.orientation.z = 0.0;
Marker.scale.z = 6.8f; // Marker.pose.orientation.w = 1.0;
Marker.color.a = 1.0; // Don't forget to set the alpha! // Marker.scale.x = 6.8f;
Marker.color.r = 0.0; // Marker.scale.y = 6.8f;
Marker.color.g = 1.0; // Marker.scale.z = 6.8f;
Marker.color.b = 0.0; // Marker.color.a = 1.0; // Don't forget to set the alpha!
//only if using a MESH_RESOURCE marker type: // Marker.color.r = 0.0;
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae"; // 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}, // cubeVisPoints = {{10.0, 10.0, 0.02}, {10, -10, 0.02},
// {50, 10, 0.02}, {50, 30, 0.02}, // {50, 10, 0.02}, {50, 30, 0.02},
@@ -168,36 +176,39 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
// {240, 90, 0.02}, {80, 110, 0.02}, // {240, 90, 0.02}, {80, 110, 0.02},
// {280, 110, 0.02}, {100, 130, 0.02}, // {280, 110, 0.02}, {100, 130, 0.02},
// {320, 130, 0.02}, {320, 150, 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}, //// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
{1184.375, 735.604, yCubesVisPos}, // float yCubesVisPos = 3.0;
// {1232.697, 715.647, yCubesVisPos}, // float skew = 5.0;
{1244.767, 784.257, yCubesVisPos} // cubeVisPoints = {
}; // {71.215, 32.694, yCubesVisPos+3},
cashedCubeVisPoints = cubeVisPoints; //// {81.668, 26.227, yCubesVisPos},
// End of added by PI // {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 // option `to enable publishing of global to IMU transformation
nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true); nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true);
@@ -772,14 +783,15 @@ void ROS1Visualizer::publish_images() {
camInfoGlob.width = exl_msg->width / 2; camInfoGlob.width = exl_msg->width / 2;
pub_cam0_info.publish(camInfoGlob); pub_cam0_info.publish(camInfoGlob);
for (auto & cubeVisPoint : cubeVisPoints){ //// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
geometry_msgs::Point tempPoint; // for (auto & cubeVisPoint : cubeVisPoints){
tempPoint.x = cubeVisPoint[0]; // geometry_msgs::Point tempPoint;
tempPoint.y = cubeVisPoint[1]; // tempPoint.x = cubeVisPoint[0];
tempPoint.z = cubeVisPoint[2]; // tempPoint.y = cubeVisPoint[1];
Marker.points.push_back(tempPoint); // tempPoint.z = cubeVisPoint[2];
} // Marker.points.push_back(tempPoint);
vis_pub.publish( Marker ); // }
// vis_pub.publish( Marker );
// pub_camera_info.publish(cameraparams); // pub_camera_info.publish(cameraparams);
// pub_camera_info_trackhist.publish(cameraparams); // pub_camera_info_trackhist.publish(cameraparams);