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

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;
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<ros::NodeHandle> nh, std::shared_
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);
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_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2);
// camPub = nh->advertise<image_transport::CameraPublisher>("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<ros::NodeHandle> 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<bool>("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);