From 3f5b448aa3cbbd695eae54795b48d85c1ce46e6b Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Mon, 23 May 2022 00:08:46 +0200 Subject: [PATCH 1/6] graph_map msgs are now in graph_map_custom_msgs --- CMakeLists.txt | 5 ++++- msg/GraphMapLocalizationMsg.msg | 3 ++- package.xml | 2 ++ src/CMakeLists.txt | 2 ++ 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2160f236..bc62261b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS grid_map_cv graph_map + graph_map_custom_msgs message_generation geometry_msgs @@ -86,7 +87,8 @@ add_message_files( generate_messages( DEPENDENCIES - geometry_msgs std_msgs ndt_map grid_map_msgs graph_map auto_complete_graph + geometry_msgs std_msgs ndt_map grid_map_msgs auto_complete_graph #graph_map + graph_map_custom_msgs ) @@ -108,6 +110,7 @@ catkin_package( LIBRARIES ${G2O_LIBS} ${vodigrex_LIBRARIES} auto_complete_graph_lib auto_complete_graph_vertexedges_lib auto_complete_graph_localization_lib # LIBRARIES auto_complete_graph CATKIN_DEPENDS message_runtime ndt_feature_finder ndt_map roscpp rospy std_msgs message_generation occupancy_grid_utils grid_map_core grid_map_ros grid_map_msgs grid_map_cv graph_map message_generation geometry_msgs std_msgs ndt_localization + graph_map_custom_msgs DEPENDS EIGEN OpenCV Boost vodigrex ) diff --git a/msg/GraphMapLocalizationMsg.msg b/msg/GraphMapLocalizationMsg.msg index 13a5fe52..463a2fcf 100644 --- a/msg/GraphMapLocalizationMsg.msg +++ b/msg/GraphMapLocalizationMsg.msg @@ -1,6 +1,7 @@ Header header #standard header information -graph_map/GraphMapMsg graph_map +#graph_map/GraphMapMsg graph_map +graph_map_custom_msgs/GraphMapMsg graph_map auto_complete_graph/LocalizationMsg[] localizations diff --git a/package.xml b/package.xml index 10df7608..3adccc0f 100644 --- a/package.xml +++ b/package.xml @@ -57,6 +57,7 @@ std_msgs nav_msgs graph_map + graph_map_custom_msgs ndt_localization occupancy_grid_utils @@ -77,6 +78,7 @@ std_msgs nav_msgs graph_map + graph_map_custom_msgs ndt_localization diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 982dc75d..e2c6ce94 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,3 +1,5 @@ +set (CMAKE_CXX_STANDARD 14) + add_subdirectory(VertexAndEdge) ## Declare a C++ library From 36ca3f9385e72acacc7993d295598926edfce344 Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Mon, 23 May 2022 20:46:35 +0200 Subject: [PATCH 2/6] Namespace fixes for graph_map -> graph_map_custom_msgs --- .../graph_map_publisher_localization.cpp | 44 +++++++++---------- 1 file changed, 20 insertions(+), 24 deletions(-) diff --git a/src/Localization/graph_map_publisher_localization.cpp b/src/Localization/graph_map_publisher_localization.cpp index 34fee462..c3699c7f 100644 --- a/src/Localization/graph_map_publisher_localization.cpp +++ b/src/Localization/graph_map_publisher_localization.cpp @@ -38,9 +38,9 @@ #include "graph_map/lidarUtils/lidar_utilities.h" -// #include "graph_map/GraphMapMsg.h" #include "auto_complete_graph/GraphMapLocalizationMsg.h" #include "graph_map/graph_map_conversions.h" +#include "graph_map_custom_msgs/GraphMapMsg.h" #include "ndt_map/NDTVectorMapMsg.h" #include "auto_complete_graph/Localization/AcgMclLocalization.hpp" @@ -130,7 +130,7 @@ Eigen::Affine3d getPose(const std::string& base_frame, } using namespace perception_oru::graph_map; -using namespace ::graph_map; +// using namespace ::graph_map; typedef message_filters::sync_policies::ApproximateTime @@ -531,8 +531,8 @@ class GraphMapFuserNode { // Publisher of graph_map message - graphmap_pub_ = - param_nh.advertise<::graph_map::GraphMapMsg>("graph_map", 50); + graphmap_pub_ = param_nh.advertise( + "graph_map", 50); graph_map_vector_ = param_nh.advertise( "graph_map_vector", 50); graphmap_localization_pub = @@ -605,11 +605,11 @@ class GraphMapFuserNode { // Save the new pose associated with the node. // assert(nb_of_node_new - 1 > 0); - // acg_localization->savePos(nb_of_node_new - //- 1); + // acg_localization->savePos(nb_of_node_new - + //1); ROS_DEBUG("PUBLISH: now"); // Publish message - ::graph_map::GraphMapMsg graphmapmsg; + graph_map_custom_msgs::GraphMapMsg graphmapmsg; perception_oru::graph_map::graphMapToMsg(*(fuser_->GetGraph()), graphmapmsg, map_link_id); @@ -624,18 +624,18 @@ class GraphMapFuserNode { count++; } - // std::cout << "PUBLISH " << graphmapmsg.nodes.size() - // << std::endl; + // std::cout << "PUBLISH " << graphmapmsg.nodes.size() << + // std::endl; // if (use_mcl_ && mcl_loaded_) { ROS_DEBUG( "*************************** > Real Localization < " "****************************"); - // std::cout << acg_localization->getLocalizations().size() - //<< " == " << nb_of_node_new << std::endl; - // assert(acg_localization->getLocalizations().size() - //== nb_of_node_new); + // std::cout << acg_localization->getLocalizations().size() << + //" == " << nb_of_node_new << std::endl; + // assert(acg_localization->getLocalizations().size() == + //nb_of_node_new); auto_complete_graph::GraphMapLocalizationMsg graphmaplocalizationmsg; @@ -657,8 +657,7 @@ class GraphMapFuserNode { // bool unstop = true; // while(nb_of_node_new >= 6 && - // unstop){ std::cout << "Keep publishing ? " << std::endl; - // std::cin + //unstop){ std::cout << "Keep publishing ? " << std::endl; std::cin //>> unstop; // graphmap_localization_pub.publish(graphmaplocalizationmsg); // @@ -716,7 +715,7 @@ class GraphMapFuserNode { ROS_INFO("INIT MCL FROM TF"); auto init_pose = getPoseTFTransform(world_link_id, laser_link_id); // std::cout << "Pose found " << pose_.matrix() << - // std::endl; + //std::endl; perception_oru::NDTMap* map; perception_oru::LazyGrid* lz; @@ -725,8 +724,8 @@ class GraphMapFuserNode { // std::cout << "GOT MAP FROM MESSAGE" << std::endl; - // ndtmcl_->changeMapAndInitializeFilter(*map, x, y, yaw, - // v_x, v_y, v_yaw, numPart); + // ndtmcl_->changeMapAndInitializeFilter(*map, x, y, yaw, v_x, + // v_y, v_yaw, numPart); double xx = init_pose.getOrigin().x(); double yy = init_pose.getOrigin().y(); @@ -758,8 +757,7 @@ class GraphMapFuserNode { double x = sensorpose_tmp.getOrigin().getX(); double y = sensorpose_tmp.getOrigin().getY(); double z = sensorpose_tmp.getOrigin().getZ(); - // std::cout << "xyz : " << x << " " << y << " " << - //z + // std::cout << "xyz : " << x << " " << y << " " << z //<< std::endl; TEST // x = 16.6; // y = 3.0; @@ -783,10 +781,8 @@ class GraphMapFuserNode { // ndtmcl_->setMotionModelCovYaw(cov_yaw_mcl); // ndtmcl_->setScalingFactorGaussian(scale_gaussian_mcl); - // std::cout << "Init at " << xx << " " << yy << " " << zz - // << - // std::endl; exit(0); ndtmcl_->InitializeNormal(xx, yy, yaw, - // initVar); + // std::cout << "Init at " << xx << " " << yy << " " << zz << + // std::endl; exit(0); ndtmcl_->InitializeNormal(xx, yy, yaw, initVar); // ndtmcl_->changeMapAndInitializeFilter(*map, resolution, // zfilter_min_, xx, yy, yaw, x_va, y_va, yaw_va, numPart); From ab18343cdac863a20adf64f5343c5fdb619a3b9a Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Mon, 23 May 2022 20:47:00 +0200 Subject: [PATCH 3/6] Compilation fixes --- .../auto_complete_graph/VertexAndEdge/VertexLandmarkNDT.hpp | 2 +- include/auto_complete_graph/VisuACG.hpp | 3 ++- src/Localization/CMakeLists.txt | 4 +++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/auto_complete_graph/VertexAndEdge/VertexLandmarkNDT.hpp b/include/auto_complete_graph/VertexAndEdge/VertexLandmarkNDT.hpp index 64192221..808d1265 100644 --- a/include/auto_complete_graph/VertexAndEdge/VertexLandmarkNDT.hpp +++ b/include/auto_complete_graph/VertexAndEdge/VertexLandmarkNDT.hpp @@ -172,4 +172,4 @@ class VertexLandmarkNDT : public g2o::VertexPointXYACG { } }; } // namespace g2o -#endif \ No newline at end of file +#endif diff --git a/include/auto_complete_graph/VisuACG.hpp b/include/auto_complete_graph/VisuACG.hpp index 5f9bf0b0..a21e51f5 100644 --- a/include/auto_complete_graph/VisuACG.hpp +++ b/include/auto_complete_graph/VisuACG.hpp @@ -227,7 +227,8 @@ class VisuAutoCompleteGraphBase { void toOcc( const AutoCompleteGraphBase& acg) { - drawPrior(acg) drawCornersNdt(acg); + drawPrior(acg); + drawCornersNdt(acg); drawAngles(acg); if (_nb_of_zone != acg.getRobotNodes().size()) { diff --git a/src/Localization/CMakeLists.txt b/src/Localization/CMakeLists.txt index 1856aaa6..d9fca46e 100644 --- a/src/Localization/CMakeLists.txt +++ b/src/Localization/CMakeLists.txt @@ -1,3 +1,5 @@ +set (CMAKE_CXX_STANDARD 14) + add_library(auto_complete_graph_localization_lib ACG_localization.cpp ACGPriorXY.cpp @@ -54,4 +56,4 @@ add_dependencies(test_prior_localization ${${PROJECT_NAME}_EXPORTED_TARGETS} ${c add_executable(publish_odom_to_tf publish_odometry_to_tf.cpp) target_link_libraries(publish_odom_to_tf ${catkin_LIBRARIES} ) -add_dependencies(publish_odom_to_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) \ No newline at end of file +add_dependencies(publish_odom_to_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) From 3101eb5327678e025d89eb8e70a914d04654f7e5 Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Wed, 8 Jun 2022 11:41:00 +0200 Subject: [PATCH 4/6] Removed calls to PlotMapType() to avoid link errors --- src/Localization/graph_map_fuser_node.cpp | 3 --- src/Localization/graph_map_publisher_localization.cpp | 3 --- 2 files changed, 6 deletions(-) diff --git a/src/Localization/graph_map_fuser_node.cpp b/src/Localization/graph_map_fuser_node.cpp index 3bbc96ed..c23da26a 100644 --- a/src/Localization/graph_map_fuser_node.cpp +++ b/src/Localization/graph_map_fuser_node.cpp @@ -387,7 +387,6 @@ class GraphMapFuserNode { m.lock(); fuser_->ProcessFrame(cloud, pose_, Tmotion); m.unlock(); - fuser_->PlotMapType(); tf::Transform Transform; tf::transformEigenToTF(pose_, Transform); tf_.sendTransform(tf::StampedTransform(Transform, tplot, world_link_id, @@ -596,7 +595,6 @@ class GraphMapFuserNode { plotPointcloud2(cloud); m.lock(); fuser_->ProcessFrame(cloud, pose_, Tmotion); - fuser_->PlotMapType(); m.unlock(); } void GTLaserPointsOdomCallbackTF( @@ -626,7 +624,6 @@ class GraphMapFuserNode { std::string("online_") + state_base_link_id, laser_link_id)); plotPointcloud2(cloud, t_stamp); fuser_->ProcessFrame(cloud, pose_, Tmotion); - fuser_->PlotMapType(); m.unlock(); } } diff --git a/src/Localization/graph_map_publisher_localization.cpp b/src/Localization/graph_map_publisher_localization.cpp index c3699c7f..a6360d43 100644 --- a/src/Localization/graph_map_publisher_localization.cpp +++ b/src/Localization/graph_map_publisher_localization.cpp @@ -1046,7 +1046,6 @@ class GraphMapFuserNode { m.lock(); fuser_->ProcessFrame(cloud, pose_, Tmotion); m.unlock(); - fuser_->PlotMapType(); tf::Transform Transform; tf::transformEigenToTF(pose_, Transform); @@ -1331,7 +1330,6 @@ class GraphMapFuserNode { plotPointcloud2(cloud); m.lock(); fuser_->ProcessFrame(cloud, pose_, Tmotion); - fuser_->PlotMapType(); m.unlock(); } void GTLaserPointsOdomCallbackTF( @@ -1361,7 +1359,6 @@ class GraphMapFuserNode { std::string("online_") + state_base_link_id, laser_link_id)); plotPointcloud2(cloud, t_stamp); fuser_->ProcessFrame(cloud, pose_, Tmotion); - fuser_->PlotMapType(); m.unlock(); } } From 54f00d52d65825243c913c96ed0b67a27ea41bb1 Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Wed, 15 Jun 2022 15:16:48 +0200 Subject: [PATCH 5/6] nicer? rviz file --- ACG_folder/acg.rviz | 412 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 412 insertions(+) create mode 100644 ACG_folder/acg.rviz diff --git a/ACG_folder/acg.rviz b/ACG_folder/acg.rviz new file mode 100644 index 00000000..b52019d2 --- /dev/null +++ b/ACG_folder/acg.rviz @@ -0,0 +1,412 @@ +Panels: + - Class: rviz/Displays + Help Height: 74 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + - /PointCloud21/Autocompute Value Bounds1 + Splitter Ratio: 0.453303009 + Tree Height: 757 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 10 + Class: rviz/Grid + Color: 220; 220; 220 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid -- 10 m + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: world + Value: true + - Alpha: 0.25 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid -- 1 m + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 500 + Reference Frame: world + Value: true + - Alpha: 0.200000003 + Cell Size: 0.125 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid -- 10 cm + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10000 + Reference Frame: world + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 10 + Frames: + All Enabled: false + base_link: + Value: true + chassis_link: + Value: true + fuser_base_link: + Value: true + laser_frame: + Value: true + map: + Value: true + mcl_tf: + Value: true + odom: + Value: true + velodyne: + Value: true + world: + Value: true + Marker Scale: 3 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + map: + {} + odom: + base_link: + chassis_link: + {} + laser_frame: + {} + velodyne: + {} + world: + fuser_base_link: + {} + mcl_tf: + {} + Update Interval: 0 + Value: true + - Alpha: 0.5 + Class: rviz/Map + Color Scheme: map + Draw Behind: true + Enabled: false + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 1 + Arrow Length: 0.300000012 + Axes Length: 0.300000012 + Axes Radius: 0.00999999978 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.0700000003 + Head Radius: 0.0299999993 + Name: PoseArray + Shaft Length: 0.230000004 + Shaft Radius: 0.00999999978 + Shape: Arrow (Flat) + Topic: /graph_node/particles + Unreliable: false + Value: true + - Alpha: 0.25 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.58415103 + Min Value: -2.03862 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 255; 255 + Color Transformer: FlatColor + Decay Time: 5 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 0 + Selectable: true + Size (Pixels): 2 + Size (m): 0.00999999978 + Style: Points + Topic: /velodyne_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.00999999978 + Style: Points + Topic: /laser_scan_with_ransac + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/prior_marker + Name: prior_marker + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/corner_ndt_marker + Name: corner_ndt_marker + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/gaussian_that_gave_corners + Name: g_that_gave_corner + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/ndt_nodes_marker + Name: ndt_nodes + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/localization_pose_markers + Name: loc_pose + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: /acg_node_localization/lastgraphmap_acg2_occ + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: /acg_node_localization/lastgraphmap_acg_occ + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: /acg_node_localization/two_occ_lastgraphmap_acg + Unreliable: false + Use Timestamp: false + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /acg_node_localization/prior_nodes_marker + Name: prior_nodes + Namespaces: + acg: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /acg_node_localization/prior_observations_markers + Name: prior_obs + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /acg_node_localization/angles_prior + Name: angles_prior + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Class: ndt_rviz/NDTVectorMap + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: NDTVectorMap + Topic: /graph_node/graph_map_vector + Unreliable: false + Value: true + - Alpha: 1 + Class: auto_complete_graph_rviz_visualisation/ACGMapsDisplay + Color: 204; 51; 204 + Enabled: true + History Length: 1 + Name: ACGMapsDisplay + Topic: "" + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 221; 221; 221 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -0.0599998012 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Scale: 30.3262024 + Target Frame: + Value: TopDownOrtho (rviz) + X: 2.53253675 + Y: -49.7623978 + Saved: + - Class: rviz/Orbit + Distance: 59.9505997 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -21.7656994 + Y: -6.6525898 + Z: -0.517731011 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.00999999978 + Pitch: 1.56979632 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.96504021 +Window Geometry: + "&Displays": + collapsed: false + "&Views": + collapsed: false + Height: 1015 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002190000038cfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006c01000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000440000038c000000e301000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002ac000000cb0000000000000000fb0000000a0049006d0061006700650100000293000000c20000000000000000fb0000000c00430061006d00650072006101000003580000001f0000000000000000fb0000000c00430061006d00650072006100000003760000001f000000000000000000000001000001100000038cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000440000038c000000c601000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000046fc0100000002fb0000000800540069006d00650000000000000007800000025e01000003fb0000000800540069006d00650100000000000004500000000000000000000004550000038c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Width: 1920 + X: 0 + Y: 0 From 556804ea051cae006e9657a52ace1cb73f355c49 Mon Sep 17 00:00:00 2001 From: Martin Magnusson Date: Wed, 15 Jun 2022 15:17:32 +0200 Subject: [PATCH 6/6] as param, instead of hard-coded --- launch/basement_publisher.launch | 4 +++- src/Localization/graph_map_publisher_localization.cpp | 8 +++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/launch/basement_publisher.launch b/launch/basement_publisher.launch index 70c8b4f2..8603141e 100644 --- a/launch/basement_publisher.launch +++ b/launch/basement_publisher.launch @@ -10,7 +10,7 @@ - + @@ -79,6 +79,8 @@ + + diff --git a/src/Localization/graph_map_publisher_localization.cpp b/src/Localization/graph_map_publisher_localization.cpp index a6360d43..46b8e187 100644 --- a/src/Localization/graph_map_publisher_localization.cpp +++ b/src/Localization/graph_map_publisher_localization.cpp @@ -185,6 +185,7 @@ class GraphMapFuserNode { std::string world_link_id, map_link_id, odometry_link_id, fuser_base_link_id, laser_link_id, init_pose_frame, gt_topic, bag_name, state_base_link_id; + std::string acg_maps_topic; double size_x, size_y, size_z, resolution, sensor_range, min_laser_range_; bool visualize, match2D, matchLaser, beHMT, useOdometry, initPoseFromGT, initPoseFromTF, initPoseSet, gt_mapping; @@ -252,6 +253,9 @@ class GraphMapFuserNode { /// topic to wait for laser scan messages, if available param_nh.param("laser_topic", laser_topic, "laser_scan"); + param_nh.param("acg_maps_topic", acg_maps_topic, + "acg_maps"); + /// only match 2ith 3dof param_nh.param("match2D", match2D, true); /// enable for LaserScan message input @@ -563,7 +567,7 @@ class GraphMapFuserNode { // std::cout << "new mcl done" << std::endl; // ndtmcl_ = boost::shared_ptr(ndtmcl); ndt_mcl_map = nh_.subscribe( - "acg_maps", 10, &GraphMapFuserNode::updateAll, this); + acg_maps_topic, 10, &GraphMapFuserNode::updateAll, this); mcl_pub_ = nh_.advertise("ndt_mcl", 10); @@ -1111,6 +1115,8 @@ class GraphMapFuserNode { ROS_DEBUG_STREAM( "NDT MCL needs to be init for the registration to start if " "you want to use both MCL and registration"); + ROS_DEBUG_STREAM("use_mcl : " << use_mcl_ << ", mcl_loaded " + << mcl_loaded_); } } }