Beacon msg

mrpt_rbpf_slam/CMakeLists.txt

@@ -8,7 +8,10 @@ IF($ENV{VERBOSE})
 endif()
 
 #find catkin packages
-find_package(catkin REQUIRED COMPONENTS roslib roscpp tf std_msgs nav_msgs sensor_msgs visualization_msgs mrpt_bridge dynamic_reconfigure)
+find_package(catkin REQUIRED COMPONENTS roslib roscpp tf 
+                                        std_msgs nav_msgs sensor_msgs 
+                                        geometry_msgs visualization_msgs 
+                                        mrpt_bridge dynamic_reconfigure)
 
 #find mrpt packages
 find_package(MRPT REQUIRED base gui obs slam)
@@ -19,9 +22,21 @@ INCLUDE_DIRECTORIES(
    ${CMAKE_CURRENT_BINARY_DIR}
 )
 
+add_message_files(
+  FILES
+  Beacons.msg
+  SingleBeacon.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 catkin_package( CATKIN_DEPENDS )
 
+
 ###########
 ## Build ##
 ###########
@@ -31,8 +46,6 @@ include_directories(
   include
 )
 
-
-
 add_executable(mrpt_rbpf_slam
     src/mrpt_rbpf_slam.cpp
     src/mrpt_rbpf_slam_wrapper.cpp

mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h

@@ -154,6 +154,7 @@ class PFslamWrapper : PFslam
   *
   */
   void vizBeacons();
+  void pubBeacons();
 
 private:
   ros::NodeHandle n_;        ///< Node Handle
@@ -182,7 +183,7 @@ class PFslamWrapper : PFslam
   std::vector<mrpt::opengl::CEllipsoidPtr> viz_beacons;
 
   ros::Publisher pub_map_, pub_metadata_, pub_Particles_, pub_Particles_Beacons_,
-      beacon_viz_pub_;  ///<publishers for map and pose particles
+      beacon_viz_pub_, pub_Beacons_;  ///<publishers for map and pose particles
 
   tf::TransformListener listenerTF_;         ///<transform listener
   tf::TransformBroadcaster tf_broadcaster_;  ///<transform broadcaster

mrpt_rbpf_slam/msg/Beacons.msg

@@ -0,0 +1 @@
+SingleBeacon[] beacons

mrpt_rbpf_slam/msg/SingleBeacon.msg

@@ -0,0 +1,2 @@
+uint32 id
+geometry_msgs/Pose pose

mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp

@@ -1,4 +1,6 @@
 #include "mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h"
+#include "mrpt_rbpf_slam/Beacons.h"
+#include "mrpt_rbpf_slam/SingleBeacon.h"
 
 PFslamWrapper::PFslamWrapper()
 {
@@ -64,6 +66,7 @@ void PFslamWrapper::init()
   pub_metadata_ = n_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
   // robot pose
   pub_Particles_ = n_.advertise<geometry_msgs::PoseArray>("particlecloud", 1, true);
+  pub_Beacons_ = n_.advertise<mrpt_rbpf_slam::Beacons>("bacons", 1, true);
   // ro particles poses
   pub_Particles_Beacons_ = n_.advertise<geometry_msgs::PoseArray>("particlecloud_beacons", 1, true);
   beacon_viz_pub_ = n_.advertise<visualization_msgs::MarkerArray>("/beacons_viz", 1);
@@ -264,6 +267,30 @@ void PFslamWrapper::publishMapPose()
   pub_Particles_.publish(poseArray);
 }
 
+void PFslamWrapper::pubBeacons() {
+
+  auto msg = mrpt_rbpf_slam::Beacons();
+
+  for (int i = 0; i < viz_beacons.size(); i++)
+  {
+    CPose3D meanPose(viz_beacons[i]->getPose());
+
+    auto beacon_pose = geometry_msgs::Pose();
+
+    beacon_pose.position.x = meanPose.x();
+    beacon_pose.position.y = meanPose.y();
+    beacon_pose.position.z = meanPose.z();
+
+    auto beacon = mrpt_rbpf_slam::SingleBeacon();
+    beacon.pose = beacon_pose;
+    beacon.id = i;
+
+    msg.beacons.push_back(beacon);
+  }
+
+  pub_Beacons_.publish(msg);
+}
+
 void PFslamWrapper::vizBeacons()
 {
   if (viz_beacons.size() == 0)
@@ -414,6 +441,7 @@ bool PFslamWrapper::rawlogPlay()
           }
           pub_Particles_Beacons_.publish(poseArrayBeacons);
           vizBeacons();
+          pubBeacons();
           viz_beacons.clear();
         }