From 775cef8588d962be7543390721e881c164c010e4 Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Mon, 28 Nov 2016 11:58:09 +0100 Subject: [PATCH 1/3] add message --- mrpt_rbpf_slam/CMakeLists.txt | 5 ++++ .../mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h | 2 +- mrpt_rbpf_slam/msg/Beacons.msg | 1 + mrpt_rbpf_slam/msg/SingleBeacon.msg | 2 ++ mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp | 26 +++++++++++++++++++ 5 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 mrpt_rbpf_slam/msg/Beacons.msg create mode 100644 mrpt_rbpf_slam/msg/SingleBeacon.msg diff --git a/mrpt_rbpf_slam/CMakeLists.txt b/mrpt_rbpf_slam/CMakeLists.txt index cd6fc01..7b6671a 100644 --- a/mrpt_rbpf_slam/CMakeLists.txt +++ b/mrpt_rbpf_slam/CMakeLists.txt @@ -19,6 +19,11 @@ INCLUDE_DIRECTORIES( ${CMAKE_CURRENT_BINARY_DIR} ) +add_message_files( + FILES + Beacons.msg + SingleBeacon.msg +) catkin_package( CATKIN_DEPENDS ) diff --git a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h b/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h index 39833f7..b500edc 100644 --- a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h +++ b/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h @@ -182,7 +182,7 @@ class PFslamWrapper : PFslam std::vector viz_beacons; ros::Publisher pub_map_, pub_metadata_, pub_Particles_, pub_Particles_Beacons_, - beacon_viz_pub_; ///("map_metadata", 1, true); // robot pose pub_Particles_ = n_.advertise("particlecloud", 1, true); + pub_Beacons_ = n_.advertise("bacons", 1, true); // ro particles poses pub_Particles_Beacons_ = n_.advertise("particlecloud_beacons", 1, true); beacon_viz_pub_ = n_.advertise("/beacons_viz", 1); @@ -264,6 +267,29 @@ void PFslamWrapper::publishMapPose() pub_Particles_.publish(poseArray); } +void PDFslamWrapper::pubBeacons() { + + auto msg = mrpt_rbpf::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::SingleBeacon(); + beacon.pose = beacon_pose; + beacon.id = i; + + msg.beacons.push_back(beacon); + } + pub_Beacons_.publish(beacon); +} + void PFslamWrapper::vizBeacons() { if (viz_beacons.size() == 0) From ac550ce2bf0e21a7c353621c21006b46f941b705 Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Mon, 28 Nov 2016 12:14:41 +0100 Subject: [PATCH 2/3] fix cmake --- mrpt_rbpf_slam/CMakeLists.txt | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/mrpt_rbpf_slam/CMakeLists.txt b/mrpt_rbpf_slam/CMakeLists.txt index 7b6671a..765300f 100644 --- a/mrpt_rbpf_slam/CMakeLists.txt +++ b/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) @@ -25,8 +28,15 @@ add_message_files( SingleBeacon.msg ) +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + catkin_package( CATKIN_DEPENDS ) + ########### ## Build ## ########### @@ -36,8 +46,6 @@ include_directories( include ) - - add_executable(mrpt_rbpf_slam src/mrpt_rbpf_slam.cpp src/mrpt_rbpf_slam_wrapper.cpp From 69e0d7df10d4ce83742ba3358b0f2775cc0a105d Mon Sep 17 00:00:00 2001 From: Wolf Vollprecht Date: Mon, 28 Nov 2016 13:16:21 +0100 Subject: [PATCH 3/3] fix --- .../include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h | 1 + mrpt_rbpf_slam/msg/SingleBeacon.msg | 2 +- mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp | 12 +++++++----- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h b/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h index b500edc..432297e 100644 --- a/mrpt_rbpf_slam/include/mrpt_rbpf_slam/mrpt_rbpf_slam_wrapper.h +++ b/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 diff --git a/mrpt_rbpf_slam/msg/SingleBeacon.msg b/mrpt_rbpf_slam/msg/SingleBeacon.msg index 8045b45..bfb63a6 100644 --- a/mrpt_rbpf_slam/msg/SingleBeacon.msg +++ b/mrpt_rbpf_slam/msg/SingleBeacon.msg @@ -1,2 +1,2 @@ -int id +uint32 id geometry_msgs/Pose pose \ No newline at end of file diff --git a/mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp b/mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp index ac69b7b..287c702 100644 --- a/mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp +++ b/mrpt_rbpf_slam/src/mrpt_rbpf_slam_wrapper.cpp @@ -66,7 +66,7 @@ void PFslamWrapper::init() pub_metadata_ = n_.advertise("map_metadata", 1, true); // robot pose pub_Particles_ = n_.advertise("particlecloud", 1, true); - pub_Beacons_ = n_.advertise("bacons", 1, true); + pub_Beacons_ = n_.advertise("bacons", 1, true); // ro particles poses pub_Particles_Beacons_ = n_.advertise("particlecloud_beacons", 1, true); beacon_viz_pub_ = n_.advertise("/beacons_viz", 1); @@ -267,9 +267,9 @@ void PFslamWrapper::publishMapPose() pub_Particles_.publish(poseArray); } -void PDFslamWrapper::pubBeacons() { +void PFslamWrapper::pubBeacons() { - auto msg = mrpt_rbpf::Beacons(); + auto msg = mrpt_rbpf_slam::Beacons(); for (int i = 0; i < viz_beacons.size(); i++) { @@ -281,13 +281,14 @@ void PDFslamWrapper::pubBeacons() { beacon_pose.position.y = meanPose.y(); beacon_pose.position.z = meanPose.z(); - auto beacon = mrpt_rbpf::SingleBeacon(); + auto beacon = mrpt_rbpf_slam::SingleBeacon(); beacon.pose = beacon_pose; beacon.id = i; msg.beacons.push_back(beacon); } - pub_Beacons_.publish(beacon); + + pub_Beacons_.publish(msg); } void PFslamWrapper::vizBeacons() @@ -440,6 +441,7 @@ bool PFslamWrapper::rawlogPlay() } pub_Particles_Beacons_.publish(poseArrayBeacons); vizBeacons(); + pubBeacons(); viz_beacons.clear(); }