Merge pull request #10 from MalcolmMielle/maptopic-as-param

Maptopic as param
MalcolmMielle · Jun 16, 2022 · ffe7b0d · ffe7b0d
2 parents 774ec8a + 556804e
commit ffe7b0d
Show file tree

Hide file tree

Showing 11 changed files with 458 additions and 37 deletions.
diff --git a/ACG_folder/acg.rviz b/ACG_folder/acg.rviz
diff --git 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/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
+#endif
diff --git a/include/auto_complete_graph/VisuACG.hpp b/include/auto_complete_graph/VisuACG.hpp
@@ -227,7 +227,8 @@ class VisuAutoCompleteGraphBase {
 
     void toOcc(
         const AutoCompleteGraphBase<Prior, VertexPrior, EdgePrior>& acg) {
-        drawPrior(acg) drawCornersNdt(acg);
+        drawPrior(acg);
+        drawCornersNdt(acg);
         drawAngles(acg);
 
         if (_nb_of_zone != acg.getRobotNodes().size()) {

diff --git a/launch/basement_publisher.launch b/launch/basement_publisher.launch
@@ -10,7 +10,7 @@
 
 <!-- //Launch basement bag file !!!!!-->
 
-<include file="$(find gustav_scripts)/URDF/urdf_basement.launch"/>
+<!-- <include file="$(find gustav_scripts)/URDF/urdf_basement.launch"/> -->
 
 <node pkg="tf" type="static_transform_publisher" name="world_odom_frame" args="0 0 0 0 0 0 /world /odom 10"/>
 <!-- <node pkg="tf" type="static_transform_publisher" name="world_laser_frame_frame" args="1 -50 0 0 0 0 /world /base_link_tmp 10"/> -->
@@ -79,6 +79,8 @@
       <param name="odometry_topic" value="/odom" />
       <param name="use_tf_listener" value="false" />
 
+      <param name="acg_maps_topic" value="/acg_node_localization/acg_maps" />
+
       <param name="max_translation_norm" value="0.4" />
       <param name="max_rotation_norm" value="0.78539816339" />
 

diff --git 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
@@ -57,6 +57,7 @@
   <build_depend>std_msgs     </build_depend>
   <build_depend>nav_msgs     </build_depend>
   <build_depend>graph_map     </build_depend>
+  <build_depend>graph_map_custom_msgs</build_depend>
   <build_depend>ndt_localization     </build_depend>
 
   <run_depend>occupancy_grid_utils</run_depend>
@@ -77,6 +78,7 @@
   <run_depend>std_msgs     </run_depend>
   <run_depend>nav_msgs     </run_depend>
   <run_depend>graph_map     </run_depend>
+  <run_depend>graph_map_custom_msgs</run_depend>
   <run_depend>ndt_localization     </run_depend>
 
 

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -1,3 +1,5 @@
+set (CMAKE_CXX_STANDARD 14)
+
 add_subdirectory(VertexAndEdge)
 
 ## Declare a C++ library

diff --git 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} )
+add_dependencies(publish_odom_to_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
diff --git 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<pcl::PointXYZ>(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
@@ -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<sensor_msgs::LaserScan,
                                                         nav_msgs::Odometry>
@@ -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<std::string>("laser_topic", laser_topic, "laser_scan");
 
+        param_nh.param<std::string>("acg_maps_topic", acg_maps_topic,
+                                    "acg_maps");
+
         /// only match 2ith 3dof
         param_nh.param("match2D", match2D, true);
         /// enable for LaserScan message input
@@ -531,8 +535,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_custom_msgs::GraphMapMsg>(
+            "graph_map", 50);
         graph_map_vector_ = param_nh.advertise<ndt_map::NDTVectorMapMsg>(
             "graph_map_vector", 50);
         graphmap_localization_pub =
@@ -563,7 +567,7 @@ class GraphMapFuserNode {
             // 		std::cout << "new mcl done" << std::endl;
             // 		ndtmcl_ = boost::shared_ptr<NDTMCL>(ndtmcl);
             ndt_mcl_map = nh_.subscribe<auto_complete_graph::ACGMaps>(
-                "acg_maps", 10, &GraphMapFuserNode::updateAll, this);
+                acg_maps_topic, 10, &GraphMapFuserNode::updateAll, this);
 
             mcl_pub_ = nh_.advertise<nav_msgs::Odometry>("ndt_mcl", 10);
 
@@ -605,11 +609,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 +628,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 +661,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 +719,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 +728,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 +761,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 +785,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);
@@ -1050,7 +1050,6 @@ class GraphMapFuserNode {
                 m.lock();
                 fuser_->ProcessFrame<pcl::PointXYZ>(cloud, pose_, Tmotion);
                 m.unlock();
-                fuser_->PlotMapType();
                 tf::Transform Transform;
                 tf::transformEigenToTF(pose_, Transform);
 
@@ -1116,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_);
             }
         }
     }
@@ -1335,7 +1336,6 @@ class GraphMapFuserNode {
         plotPointcloud2(cloud);
         m.lock();
         fuser_->ProcessFrame(cloud, pose_, Tmotion);
-        fuser_->PlotMapType();
         m.unlock();
     }
     void GTLaserPointsOdomCallbackTF(
@@ -1365,7 +1365,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();
         }
     }
-Original file line number
+Diff line change
@@ Expand Up / @@ -172,4 +172,4 @@ class VertexLandmarkNDT : public g2o::VertexPointXYACG { @@
         }
     };
     }  // namespace g2o
-    #endif
+    #endif