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

2022-08-17 15:43:07 +03:00
parent f8843bf0fc
commit b8ee6672d1
4 changed files with 166 additions and 59 deletions
--- a/ov_msckf/src/render_cubes.cpp
+++ b/ov_msckf/src/render_cubes.cpp
@@ -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();
+}