From 888f80699ed8466e9528a8d78c6e3c825017cdb0 Mon Sep 17 00:00:00 2001 From: Hriday Bavle Date: Mon, 13 May 2024 15:35:37 +0200 Subject: [PATCH] [Feat] Change s_graphs_msgs->situational_graphs_msgs --- .gitmodules | 8 +- .rosinstall_ros2 | 4 +- s_graphs/CMakeLists.txt | 4 +- s_graphs/apps/floor_plan_node.cpp | 65 +++++----- s_graphs/apps/room_segmentation_node.cpp | 45 ++++--- s_graphs/apps/s_graphs_node.cpp | 119 ++++++++++-------- s_graphs/apps/scan_matching_odometry_node.cpp | 13 +- .../include/s_graphs/backend/floor_mapper.hpp | 10 +- .../include/s_graphs/backend/plane_mapper.hpp | 53 ++++---- .../include/s_graphs/backend/room_mapper.hpp | 12 +- .../include/s_graphs/backend/wall_mapper.hpp | 23 ++-- .../include/s_graphs/common/plane_utils.hpp | 25 ++-- .../include/s_graphs/common/room_utils.hpp | 10 +- .../s_graphs/frontend/floor_analyzer.hpp | 10 +- .../s_graphs/frontend/room_analyzer.hpp | 33 ++--- .../visualization/graph_publisher.hpp | 20 ++- s_graphs/package.xml | 2 +- .../s_graphs/backend/finite_room_mapper.cpp | 8 +- .../src/s_graphs/backend/floor_mapper.cpp | 6 +- .../s_graphs/backend/infinite_room_mapper.cpp | 2 +- s_graphs/src/s_graphs/backend/wall_mapper.cpp | 4 +- s_graphs/src/s_graphs/common/plane_utils.cpp | 27 ++-- .../src/s_graphs/frontend/floor_analyzer.cpp | 10 +- .../src/s_graphs/frontend/room_analyzer.cpp | 20 +-- s_graphs/test/testRoom.cpp | 10 +- s_graphs/test/testRoomCentreCompute.cpp | 8 +- utils/msgs/s_graphs_msgs | 1 - utils/msgs/situational_graphs_msgs | 1 + utils/reasoning/situational_graphs_reasoning | 2 +- 29 files changed, 295 insertions(+), 260 deletions(-) delete mode 160000 utils/msgs/s_graphs_msgs create mode 160000 utils/msgs/situational_graphs_msgs diff --git a/.gitmodules b/.gitmodules index 4dae240..d4385b5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,10 +9,6 @@ path = utils/ndt_omp url = https://github.com/koide3/ndt_omp.git branch = master -[submodule "utils/msgs/s_graphs_msgs"] - path = utils/msgs/s_graphs_msgs - url = https://github.com/snt-arg/s_graphs_msgs.git - branch = main [submodule "utils/reasoning/situational_graphs_reasoning"] path = utils/reasoning/situational_graphs_reasoning url = https://github.com/snt-arg/situational_graphs_reasoning.git @@ -29,3 +25,7 @@ path = utils/msgs/situational_graphs_reasoning_msgs url = https://github.com/snt-arg/situational_graphs_reasoning_msgs.git branch = main +[submodule "utils/msgs/situational_graphs_msgs"] + path = utils/msgs/situational_graphs_msgs + url = https://github.com/snt-arg/situational_graphs_msgs + branch = main diff --git a/.rosinstall_ros2 b/.rosinstall_ros2 index d1df470..1a14cd4 100644 --- a/.rosinstall_ros2 +++ b/.rosinstall_ros2 @@ -19,7 +19,7 @@ repositories: type: git url: https://github.com/snt-arg/situational_graphs_reasoning_msgs.git version: main - s_graphs_utils/msgs/s_graphs_msgs: + s_graphs_utils/msgs/situational_graphs_msgs: type: git - url: https://github.com/snt-arg/s_graphs_msgs.git + url: https://github.com/snt-arg/situational_graphs_msgs.git version: main diff --git a/s_graphs/CMakeLists.txt b/s_graphs/CMakeLists.txt index e19436e..bf77d49 100644 --- a/s_graphs/CMakeLists.txt +++ b/s_graphs/CMakeLists.txt @@ -44,7 +44,7 @@ find_package(geographic_msgs REQUIRED) find_package(situational_graphs_reasoning_msgs REQUIRED) find_package(backward_ros REQUIRED) find_package(rviz_visual_tools REQUIRED) -find_package(s_graphs_msgs REQUIRED) +find_package(situational_graphs_msgs REQUIRED) find_package(PCL REQUIRED) @@ -99,7 +99,7 @@ set(DEPENDENCIES nmea_msgs geodesy rviz_visual_tools - s_graphs_msgs) + situational_graphs_msgs) SET(G2O_COMMON_LIBS ${PCL_LIBRARIES} diff --git a/s_graphs/apps/floor_plan_node.cpp b/s_graphs/apps/floor_plan_node.cpp index e32ea82..8950e4c 100644 --- a/s_graphs/apps/floor_plan_node.cpp +++ b/s_graphs/apps/floor_plan_node.cpp @@ -39,11 +39,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/point_clouds.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/point_clouds.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.h" namespace s_graphs { @@ -83,14 +83,17 @@ class FloorPlanNode : public rclcpp::Node { 1, std::bind( &FloorPlanNode::skeleton_graph_callback, this, std::placeholders::_1)); - map_planes_sub = this->create_subscription( - "s_graphs/all_map_planes", - 100, - std::bind(&FloorPlanNode::map_planes_callback, this, std::placeholders::_1)); - - all_rooms_data_pub = this->create_publisher( - "floor_plan/all_rooms_data", 1); - floor_data_pub = this->create_publisher( + map_planes_sub = + this->create_subscription( + "s_graphs/all_map_planes", + 100, + std::bind( + &FloorPlanNode::map_planes_callback, this, std::placeholders::_1)); + + all_rooms_data_pub = + this->create_publisher( + "floor_plan/all_rooms_data", 1); + floor_data_pub = this->create_publisher( "floor_plan/floor_data", 1); floor_plane_timer = create_wall_timer( @@ -122,15 +125,15 @@ class FloorPlanNode : public rclcpp::Node { * */ void map_planes_callback( - const s_graphs_msgs::msg::PlanesData::SharedPtr map_planes_msg) { + const situational_graphs_msgs::msg::PlanesData::SharedPtr map_planes_msg) { std::lock_guard lock(map_plane_mutex); x_vert_plane_queue.push_back(map_planes_msg->x_planes); y_vert_plane_queue.push_back(map_planes_msg->y_planes); } void flush_map_planes( - std::vector& current_x_vert_planes, - std::vector& current_y_vert_planes) { + std::vector& current_x_vert_planes, + std::vector& current_y_vert_planes) { std::lock_guard lock(map_plane_mutex); for (const auto& x_map_planes_msg : x_vert_plane_queue) { for (const auto& x_map_plane : x_map_planes_msg) { @@ -158,7 +161,7 @@ class FloorPlanNode : public rclcpp::Node { } void floor_plan_callback() { - std::vector current_x_vert_planes, + std::vector current_x_vert_planes, current_y_vert_planes; flush_map_planes(current_x_vert_planes, current_y_vert_planes); @@ -182,9 +185,10 @@ class FloorPlanNode : public rclcpp::Node { } void extract_rooms( - const std::vector& current_x_vert_planes, - const std::vector& current_y_vert_planes) { - std::vector room_candidates_vec; + const std::vector& current_x_vert_planes, + const std::vector& + current_y_vert_planes) { + std::vector room_candidates_vec; std::vector::Ptr> curr_cloud_clusters = room_analyzer->extract_cloud_clusters(); @@ -207,7 +211,7 @@ class FloorPlanNode : public rclcpp::Node { room_analyzer->perform_room_segmentation( room_info, cloud_cluster, room_candidates_vec, current_cloud_marker); - s_graphs_msgs::msg::RoomsData room_candidates_msg; + situational_graphs_msgs::msg::RoomsData room_candidates_msg; room_candidates_msg.header.stamp = this->now(); room_candidates_msg.rooms = room_candidates_vec; all_rooms_data_pub->publish(room_candidates_msg); @@ -215,9 +219,10 @@ class FloorPlanNode : public rclcpp::Node { } void extract_floors( - const std::vector& current_x_vert_planes, - const std::vector& current_y_vert_planes) { - std::vector floor_plane_candidates_vec; + const std::vector& current_x_vert_planes, + const std::vector& + current_y_vert_planes) { + std::vector floor_plane_candidates_vec; floor_analyzer->perform_floor_segmentation( current_x_vert_planes, current_y_vert_planes, floor_plane_candidates_vec); @@ -228,7 +233,7 @@ class FloorPlanNode : public rclcpp::Node { floor_plane_candidates_vec[2], floor_plane_candidates_vec[3]); - s_graphs_msgs::msg::RoomData floor_data_msg; + situational_graphs_msgs::msg::RoomData floor_data_msg; floor_data_msg.header.stamp = this->now(); floor_data_msg.id = 0; floor_data_msg.x_planes.push_back(floor_plane_candidates_vec[0]); @@ -243,9 +248,11 @@ class FloorPlanNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr skeleton_graph_sub; - rclcpp::Subscription::SharedPtr map_planes_sub; - rclcpp::Publisher::SharedPtr all_rooms_data_pub; - rclcpp::Publisher::SharedPtr floor_data_pub; + rclcpp::Subscription::SharedPtr + map_planes_sub; + rclcpp::Publisher::SharedPtr + all_rooms_data_pub; + rclcpp::Publisher::SharedPtr floor_data_pub; private: rclcpp::TimerBase::SharedPtr floor_plane_timer; @@ -257,7 +264,7 @@ class FloorPlanNode : public rclcpp::Node { std::unique_ptr floor_analyzer; std::mutex map_plane_mutex; - std::deque> x_vert_plane_queue, + std::deque> x_vert_plane_queue, y_vert_plane_queue; }; diff --git a/s_graphs/apps/room_segmentation_node.cpp b/s_graphs/apps/room_segmentation_node.cpp index 9ea33aa..e101976 100644 --- a/s_graphs/apps/room_segmentation_node.cpp +++ b/s_graphs/apps/room_segmentation_node.cpp @@ -60,7 +60,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "pcl_conversions/pcl_conversions.h" #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "std_msgs/msg/color_rgba.hpp" #include "tf2_ros/transform_listener.h" #include "visualization_msgs/msg/marker_array.hpp" @@ -108,17 +108,19 @@ class RoomSegmentationNode : public rclcpp::Node { std::bind(&RoomSegmentationNode::skeleton_graph_callback, this, std::placeholders::_1)); - map_planes_sub = this->create_subscription( - "s_graphs/map_planes", - 100, - std::bind( - &RoomSegmentationNode::map_planes_callback, this, std::placeholders::_1)); + map_planes_sub = + this->create_subscription( + "s_graphs/map_planes", + 100, + std::bind(&RoomSegmentationNode::map_planes_callback, + this, + std::placeholders::_1)); cluster_cloud_pub = this->create_publisher( "room_segmentation/cluster_cloud", 1); cluster_clouds_pub = this->create_publisher( "room_segmentation/cluster_clouds", 1); - room_data_pub = this->create_publisher( + room_data_pub = this->create_publisher( "room_segmentation/room_data", 1); room_centers_pub = this->create_publisher( "room_segmentation/room_centers", 1); @@ -138,7 +140,7 @@ class RoomSegmentationNode : public rclcpp::Node { } void room_detection_callback() { - std::vector current_x_vert_planes, + std::vector current_x_vert_planes, current_y_vert_planes; flush_map_planes(current_x_vert_planes, current_y_vert_planes); @@ -165,15 +167,15 @@ class RoomSegmentationNode : public rclcpp::Node { * @param map_planes_msg */ void map_planes_callback( - const s_graphs_msgs::msg::PlanesData::SharedPtr map_planes_msg) { + const situational_graphs_msgs::msg::PlanesData::SharedPtr map_planes_msg) { std::lock_guard lock(map_plane_mutex); x_vert_plane_queue.push_back(map_planes_msg->x_planes); y_vert_plane_queue.push_back(map_planes_msg->y_planes); } void flush_map_planes( - std::vector& current_x_vert_planes, - std::vector& current_y_vert_planes) { + std::vector& current_x_vert_planes, + std::vector& current_y_vert_planes) { std::lock_guard lock(map_plane_mutex); for (const auto& x_map_planes_msg : x_vert_plane_queue) { for (const auto& x_map_plane : x_map_planes_msg) { @@ -215,11 +217,12 @@ class RoomSegmentationNode : public rclcpp::Node { * @brief extract clusters with its centers from the skeletal cloud * */ - void extract_rooms(std::vector current_x_vert_planes, - std::vector current_y_vert_planes) { + void extract_rooms( + std::vector current_x_vert_planes, + std::vector current_y_vert_planes) { int room_cluster_counter = 0; visualization_msgs::msg::MarkerArray refined_skeleton_marker_array; - std::vector room_candidates_vec; + std::vector room_candidates_vec; pcl::PointCloud::Ptr cloud_visualizer( new pcl::PointCloud); pcl::PointCloud::Ptr cloud_hull_visualizer( @@ -271,7 +274,7 @@ class RoomSegmentationNode : public rclcpp::Node { cluster_id++; } - s_graphs_msgs::msg::RoomsData room_candidates_msg; + situational_graphs_msgs::msg::RoomsData room_candidates_msg; room_candidates_msg.header.stamp = this->now(); room_candidates_msg.rooms = room_candidates_vec; room_data_pub->publish(room_candidates_msg); @@ -284,7 +287,7 @@ class RoomSegmentationNode : public rclcpp::Node { cluster_cloud_pub->publish(cloud_cluster_msg); } - void viz_room_centers(s_graphs_msgs::msg::RoomsData room_vec) { + void viz_room_centers(situational_graphs_msgs::msg::RoomsData room_vec) { visualization_msgs::msg::Marker room_marker; room_marker.pose.orientation.w = 1.0; room_marker.scale.x = 0.5; @@ -327,11 +330,12 @@ class RoomSegmentationNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr skeleton_graph_sub; - rclcpp::Subscription::SharedPtr map_planes_sub; + rclcpp::Subscription::SharedPtr + map_planes_sub; rclcpp::Publisher::SharedPtr cluster_cloud_pub; rclcpp::Publisher::SharedPtr cluster_clouds_pub; - rclcpp::Publisher::SharedPtr room_data_pub; + rclcpp::Publisher::SharedPtr room_data_pub; rclcpp::Publisher::SharedPtr room_centers_pub; /* private variables */ @@ -339,8 +343,9 @@ class RoomSegmentationNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr room_detection_timer; std::mutex map_plane_mutex; - std::vector x_vert_plane_vec, y_vert_plane_vec; - std::deque> x_vert_plane_queue, + std::vector x_vert_plane_vec, + y_vert_plane_vec; + std::deque> x_vert_plane_queue, y_vert_plane_queue; std::unique_ptr room_analyzer; diff --git a/s_graphs/apps/s_graphs_node.cpp b/s_graphs/apps/s_graphs_node.cpp index 40b18ff..8db82e3 100644 --- a/s_graphs/apps/s_graphs_node.cpp +++ b/s_graphs/apps/s_graphs_node.cpp @@ -79,21 +79,20 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "nmea_msgs/msg/sentence.hpp" #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/floor_coeffs.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/point_clouds.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" -#include "s_graphs_msgs/msg/wall_data.hpp" -#include "s_graphs_msgs/msg/walls_data.hpp" -#include "s_graphs_msgs/srv/dump_graph.hpp" -#include "s_graphs_msgs/srv/load_graph.hpp" -#include "s_graphs_msgs/srv/save_map.hpp" -#include "sensor_msgs/msg/imu.h" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/floor_coeffs.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/point_clouds.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/wall_data.hpp" +#include "situational_graphs_msgs/msg/walls_data.hpp" +#include "situational_graphs_msgs/srv/dump_graph.hpp" +#include "situational_graphs_msgs/srv/load_graph.hpp" +#include "situational_graphs_msgs/srv/save_map.hpp" #include "std_msgs/msg/color_rgba.hpp" #include "tf2_eigen/tf2_eigen.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -229,17 +228,17 @@ class SGraphsNode : public rclcpp::Node { std::bind(&SGraphsNode::imu_callback, this, std::placeholders::_1), sub_opt); - room_data_sub = this->create_subscription( + room_data_sub = this->create_subscription( "room_segmentation/room_data", 1, std::bind(&SGraphsNode::room_data_callback, this, std::placeholders::_1), sub_opt); - wall_data_sub = this->create_subscription( + wall_data_sub = this->create_subscription( "wall_segmentation/wall_data", 1, std::bind(&SGraphsNode::wall_data_callback, this, std::placeholders::_1), sub_opt); - floor_data_sub = this->create_subscription( + floor_data_sub = this->create_subscription( "floor_plan/floor_data", 1, std::bind(&SGraphsNode::floor_data_callback, this, std::placeholders::_1), @@ -280,28 +279,30 @@ class SGraphsNode : public rclcpp::Node { map_points_pub = this->create_publisher( "s_graphs/map_points", 1, pub_opt); - map_planes_pub = this->create_publisher( + map_planes_pub = this->create_publisher( "s_graphs/map_planes", 1, pub_opt); - all_map_planes_pub = this->create_publisher( - "s_graphs/all_map_planes", 1, pub_opt); + all_map_planes_pub = + this->create_publisher( + "s_graphs/all_map_planes", 1, pub_opt); read_until_pub = this->create_publisher( "s_graphs/read_until", 32, pub_opt); graph_pub = this->create_publisher( "s_graphs/graph_structure", 32, pub_opt); - dump_service_server = this->create_service( + dump_service_server = this->create_service( "s_graphs/dump", std::bind(&SGraphsNode::dump_service, this, std::placeholders::_1, std::placeholders::_2)); - save_map_service_server = this->create_service( - "s_graphs/save_map", - std::bind(&SGraphsNode::save_map_service, - this, - std::placeholders::_1, - std::placeholders::_2)); - load_service_server = this->create_service( + save_map_service_server = + this->create_service( + "s_graphs/save_map", + std::bind(&SGraphsNode::save_map_service, + this, + std::placeholders::_1, + std::placeholders::_2)); + load_service_server = this->create_service( "s_graphs/load", std::bind(&SGraphsNode::load_service, this, @@ -578,7 +579,7 @@ class SGraphsNode : public rclcpp::Node { } void floor_data_callback( - const s_graphs_msgs::msg::RoomData::SharedPtr floor_data_msg) { + const situational_graphs_msgs::msg::RoomData::SharedPtr floor_data_msg) { std::lock_guard lock(floor_data_mutex); floor_data_queue.push_back(*floor_data_msg); } @@ -608,7 +609,8 @@ class SGraphsNode : public rclcpp::Node { * @brief get the room data from room segmentation module * */ - void room_data_callback(const s_graphs_msgs::msg::RoomsData::SharedPtr rooms_msg) { + void room_data_callback( + const situational_graphs_msgs::msg::RoomsData::SharedPtr rooms_msg) { std::lock_guard lock(room_data_queue_mutex); room_data_queue.push_back(*rooms_msg); // std::cout << "pre_room_data_vec size :" << pre_room_data_vec.size() << std::endl; @@ -828,11 +830,12 @@ class SGraphsNode : public rclcpp::Node { return true; } - void wall_data_callback(const s_graphs_msgs::msg::WallsData::SharedPtr walls_msg) { + void wall_data_callback( + const situational_graphs_msgs::msg::WallsData::SharedPtr walls_msg) { for (int j = 0; j < walls_msg->walls.size(); j++) { - std::vector x_planes_msg = + std::vector x_planes_msg = walls_msg->walls[j].x_planes; - std::vector y_planes_msg = + std::vector y_planes_msg = walls_msg->walls[j].y_planes; if (x_planes_msg.size() != 2 && y_planes_msg.size() != 2) continue; @@ -1323,13 +1326,13 @@ class SGraphsNode : public rclcpp::Node { } } - s_graphs_msgs::msg::PlanesData vert_planes_data; + situational_graphs_msgs::msg::PlanesData vert_planes_data; vert_planes_data.header.stamp = keyframes.rbegin()->second->stamp; for (const auto& unique_x_plane_id : unique_x_plane_ids) { auto local_x_vert_plane = x_vert_planes_snapshot.find(unique_x_plane_id.first); if (local_x_vert_plane == x_vert_planes_snapshot.end()) continue; - s_graphs_msgs::msg::PlaneData plane_data; + situational_graphs_msgs::msg::PlaneData plane_data; Eigen::Vector4d mapped_plane_coeffs; mapped_plane_coeffs = (local_x_vert_plane->second).plane_node->estimate().coeffs(); @@ -1355,7 +1358,7 @@ class SGraphsNode : public rclcpp::Node { auto local_y_vert_plane = y_vert_planes_snapshot.find(unique_y_plane_id.first); if (local_y_vert_plane == y_vert_planes_snapshot.end()) continue; - s_graphs_msgs::msg::PlaneData plane_data; + situational_graphs_msgs::msg::PlaneData plane_data; Eigen::Vector4d mapped_plane_coeffs; mapped_plane_coeffs = (local_y_vert_plane->second).plane_node->estimate().coeffs(); @@ -1388,10 +1391,10 @@ class SGraphsNode : public rclcpp::Node { const std::vector& y_vert_planes_snapshot) { if (keyframes.empty()) return; - s_graphs_msgs::msg::PlanesData vert_planes_data; + situational_graphs_msgs::msg::PlanesData vert_planes_data; vert_planes_data.header.stamp = keyframes.rbegin()->second->stamp; for (const auto& x_vert_plane : x_vert_planes_snapshot) { - s_graphs_msgs::msg::PlaneData plane_data; + situational_graphs_msgs::msg::PlaneData plane_data; Eigen::Vector4d mapped_plane_coeffs; mapped_plane_coeffs = (x_vert_plane).plane_node->estimate().coeffs(); // correct_plane_direction(PlaneUtils::plane_class::X_VERT_PLANE, @@ -1417,7 +1420,7 @@ class SGraphsNode : public rclcpp::Node { } for (const auto& y_vert_plane : y_vert_planes_snapshot) { - s_graphs_msgs::msg::PlaneData plane_data; + situational_graphs_msgs::msg::PlaneData plane_data; Eigen::Vector4d mapped_plane_coeffs; mapped_plane_coeffs = (y_vert_plane).plane_node->estimate().coeffs(); // correct_plane_direction(PlaneUtils::plane_class::Y_VERT_PLANE, @@ -1463,8 +1466,9 @@ class SGraphsNode : public rclcpp::Node { * @param res * @return */ - bool dump_service(const std::shared_ptr req, - std::shared_ptr res) { + bool dump_service( + const std::shared_ptr req, + std::shared_ptr res) { std::lock_guard lock(graph_mutex); std::string directory = req->destination; @@ -1529,8 +1533,9 @@ class SGraphsNode : public rclcpp::Node { * @param res * @return */ - bool save_map_service(const std::shared_ptr req, - std::shared_ptr res) { + bool save_map_service( + const std::shared_ptr req, + std::shared_ptr res) { std::vector snapshot; snapshot = keyframes_snapshot; @@ -1563,8 +1568,9 @@ class SGraphsNode : public rclcpp::Node { return true; } - bool load_service(const std::shared_ptr req, - std::shared_ptr res) { + bool load_service( + const std::shared_ptr req, + std::shared_ptr res) { std::string directory = req->destination; std::vector keyframe_directories, y_planes_directories, x_planes_directories, room_directories; @@ -1764,9 +1770,12 @@ class SGraphsNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr navsat_sub; rclcpp::Subscription::SharedPtr raw_odom_sub; rclcpp::Subscription::SharedPtr imu_sub; - rclcpp::Subscription::SharedPtr room_data_sub; - rclcpp::Subscription::SharedPtr wall_data_sub; - rclcpp::Subscription::SharedPtr floor_data_sub; + rclcpp::Subscription::SharedPtr + room_data_sub; + rclcpp::Subscription::SharedPtr + wall_data_sub; + rclcpp::Subscription::SharedPtr + floor_data_sub; rclcpp::Subscription::SharedPtr point_cloud_sub; rclcpp::Subscription::SharedPtr init_odom2map_sub, map_2map_transform_sub; @@ -1785,17 +1794,21 @@ class SGraphsNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_path_corrected_pub; rclcpp::Publisher::SharedPtr read_until_pub; rclcpp::Publisher::SharedPtr map_points_pub; - rclcpp::Publisher::SharedPtr map_planes_pub; - rclcpp::Publisher::SharedPtr all_map_planes_pub; + rclcpp::Publisher::SharedPtr map_planes_pub; + rclcpp::Publisher::SharedPtr + all_map_planes_pub; rclcpp::Publisher::SharedPtr graph_pub; std::shared_ptr tf_listener{nullptr}; std::unique_ptr tf_buffer; std::unique_ptr odom2map_broadcaster; - rclcpp::Service::SharedPtr dump_service_server; - rclcpp::Service::SharedPtr load_service_server; - rclcpp::Service::SharedPtr save_map_service_server; + rclcpp::Service::SharedPtr + dump_service_server; + rclcpp::Service::SharedPtr + load_service_server; + rclcpp::Service::SharedPtr + save_map_service_server; // odom queue std::mutex odom_queue_mutex; @@ -1866,8 +1879,8 @@ class SGraphsNode : public rclcpp::Node { // room data queue std::mutex room_data_queue_mutex, floor_data_mutex; - std::deque room_data_queue; - std::deque floor_data_queue; + std::deque room_data_queue; + std::deque floor_data_queue; // for map cloud generation std::atomic_bool graph_updated; diff --git a/s_graphs/apps/scan_matching_odometry_node.cpp b/s_graphs/apps/scan_matching_odometry_node.cpp index e7ba2fd..ea1a4c4 100644 --- a/s_graphs/apps/scan_matching_odometry_node.cpp +++ b/s_graphs/apps/scan_matching_odometry_node.cpp @@ -34,7 +34,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include #include #include -#include +#include #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -90,8 +90,9 @@ class ScanMatchingOdometryNode : public rclcpp::Node { odom_pub = this->create_publisher("odom", 32); trans_pub = this->create_publisher( "scan_matching_odometry/transform", 32); - status_pub = this->create_publisher( - "scan_matching_odometry/status", 8); + status_pub = + this->create_publisher( + "scan_matching_odometry/status", 8); aligned_points_pub = this->create_publisher("aligned_points", 32); } @@ -461,8 +462,7 @@ class ScanMatchingOdometryNode : public rclcpp::Node { if (status_pub->get_subscription_count() == 0) { return; } - - s_graphs_msgs::msg::ScanMatchingStatus status; + situational_graphs_msgs::msg::ScanMatchingStatus status; status.header.frame_id = frame_id; status.header.stamp = stamp; status.has_converged = registration->hasConverged(); @@ -510,7 +510,8 @@ class ScanMatchingOdometryNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr odom_pub; rclcpp::Publisher::SharedPtr trans_pub; rclcpp::Publisher::SharedPtr aligned_points_pub; - rclcpp::Publisher::SharedPtr status_pub; + rclcpp::Publisher::SharedPtr + status_pub; rclcpp::Publisher::SharedPtr read_until_pub; std::shared_ptr tf_listener{nullptr}; diff --git a/s_graphs/include/s_graphs/backend/floor_mapper.hpp b/s_graphs/include/s_graphs/backend/floor_mapper.hpp index f0f2df8..64d5463 100644 --- a/s_graphs/include/s_graphs/backend/floor_mapper.hpp +++ b/s_graphs/include/s_graphs/backend/floor_mapper.hpp @@ -66,8 +66,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace s_graphs { @@ -99,7 +99,7 @@ class FloorMapper { */ void lookup_floors( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, std::unordered_map& floors_vec, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, @@ -118,7 +118,7 @@ class FloorMapper { */ void factor_floor_node( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, std::unordered_map& floors_vec, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, @@ -137,7 +137,7 @@ class FloorMapper { void update_floor_node( std::shared_ptr& graph_slam, g2o::VertexFloor* floor_node, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, const std::unordered_map& y_infinite_rooms); diff --git a/s_graphs/include/s_graphs/backend/plane_mapper.hpp b/s_graphs/include/s_graphs/backend/plane_mapper.hpp index 4fa6dab..55a7eb4 100644 --- a/s_graphs/include/s_graphs/backend/plane_mapper.hpp +++ b/s_graphs/include/s_graphs/backend/plane_mapper.hpp @@ -50,11 +50,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace s_graphs { @@ -86,7 +86,8 @@ class PlaneMapper { * @param hort_planes */ void map_extracted_planes( - std::shared_ptr& graph_slam, KeyFrame::Ptr keyframe, + std::shared_ptr& graph_slam, + KeyFrame::Ptr keyframe, const std::vector::Ptr>& extracted_cloud_vec, std::unordered_map& x_vert_planes, std::unordered_map& y_vert_planes, @@ -99,8 +100,8 @@ class PlaneMapper { * @param det_plane_body_frame * @return */ - g2o::Plane3D convert_plane_to_map_frame( - const KeyFrame::Ptr& keyframe, const g2o::Plane3D& det_plane_body_frame); + g2o::Plane3D convert_plane_to_map_frame(const KeyFrame::Ptr& keyframe, + const g2o::Plane3D& det_plane_body_frame); /** * @brief @@ -126,13 +127,13 @@ class PlaneMapper { * @param hort_planes * @return */ - int associate_plane( - const int& plane_type, const KeyFrame::Ptr& keyframe, - const g2o::Plane3D& det_plane, - const pcl::PointCloud::Ptr& cloud_seg_body, - const std::unordered_map& x_vert_planes, - const std::unordered_map& y_vert_planes, - const std::unordered_map& hort_planes); + int associate_plane(const int& plane_type, + const KeyFrame::Ptr& keyframe, + const g2o::Plane3D& det_plane, + const pcl::PointCloud::Ptr& cloud_seg_body, + const std::unordered_map& x_vert_planes, + const std::unordered_map& y_vert_planes, + const std::unordered_map& hort_planes); private: /** @@ -146,12 +147,12 @@ class PlaneMapper { * @param hort_planes * @return */ - int add_planes_to_graph( - std::shared_ptr& graph_slam, KeyFrame::Ptr& keyframe, - const g2o::Plane3D& det_plane_body_frame, - std::unordered_map& x_vert_planes, - std::unordered_map& y_vert_planes, - std::unordered_map& hort_planes); + int add_planes_to_graph(std::shared_ptr& graph_slam, + KeyFrame::Ptr& keyframe, + const g2o::Plane3D& det_plane_body_frame, + std::unordered_map& x_vert_planes, + std::unordered_map& y_vert_planes, + std::unordered_map& hort_planes); /** * @brief @@ -166,7 +167,8 @@ class PlaneMapper { * @param hort_planes * @return */ - int sort_planes(std::shared_ptr& graph_slam, const int& plane_type, + int sort_planes(std::shared_ptr& graph_slam, + const int& plane_type, KeyFrame::Ptr& keyframe, const g2o::Plane3D& det_plane_map_frame, const g2o::Plane3D& det_plane_body_frame, @@ -188,7 +190,8 @@ class PlaneMapper { * @return */ int factor_planes(std::shared_ptr& graph_slam, - const int& plane_type, KeyFrame::Ptr& keyframe, + const int& plane_type, + KeyFrame::Ptr& keyframe, const g2o::Plane3D& det_plane_map_frame, const g2o::Plane3D& det_plane_body_frame, std::unordered_map& x_vert_planes, @@ -206,10 +209,12 @@ class PlaneMapper { * @param found_room * @param plane_id_pair */ - void retrieve_plane_properties(const int& plane_type, const int& plane_id, + void retrieve_plane_properties(const int& plane_type, + const int& plane_id, const KeyFrame::Ptr& keyframe, const g2o::Plane3D& det_plane_map_frame, - bool& found_infinite_room, bool& found_room, + bool& found_infinite_room, + bool& found_room, plane_data_list& plane_id_pair); private: diff --git a/s_graphs/include/s_graphs/backend/room_mapper.hpp b/s_graphs/include/s_graphs/backend/room_mapper.hpp index b9666e7..294ae74 100644 --- a/s_graphs/include/s_graphs/backend/room_mapper.hpp +++ b/s_graphs/include/s_graphs/backend/room_mapper.hpp @@ -49,7 +49,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace s_graphs { @@ -191,7 +191,7 @@ class InfiniteRoomMapper : public MapperUtils { bool lookup_infinite_rooms( std::shared_ptr& graph_slam, const int& plane_type, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& x_vert_planes, const std::unordered_map& y_vert_planes, std::deque>& dupl_x_vert_planes, @@ -322,7 +322,7 @@ class FiniteRoomMapper : public MapperUtils { */ bool lookup_rooms( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& x_vert_planes, const std::unordered_map& y_vert_planes, std::deque>& dupl_x_vert_planes, @@ -427,7 +427,7 @@ class FiniteRoomMapper : public MapperUtils { */ void map_room_from_existing_infinite_rooms( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_x_infinite_room, const s_graphs::InfiniteRooms& matched_y_infinite_room, const Eigen::Isometry3d& room_center, @@ -455,7 +455,7 @@ class FiniteRoomMapper : public MapperUtils { */ void map_room_from_existing_x_infinite_room( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_x_infinite_room, const Eigen::Isometry3d& room_center, std::unordered_map& rooms_vec, @@ -482,7 +482,7 @@ class FiniteRoomMapper : public MapperUtils { */ void map_room_from_existing_y_infinite_room( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_y_infinite_room, const Eigen::Isometry3d& room_center, std::unordered_map& rooms_vec, diff --git a/s_graphs/include/s_graphs/backend/wall_mapper.hpp b/s_graphs/include/s_graphs/backend/wall_mapper.hpp index c9efd77..59fc0c8 100644 --- a/s_graphs/include/s_graphs/backend/wall_mapper.hpp +++ b/s_graphs/include/s_graphs/backend/wall_mapper.hpp @@ -49,11 +49,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace s_graphs { @@ -81,13 +81,14 @@ class WallMapper { * @param x_vert_planes * @param y_vert_planes */ - void factor_wall(const std::shared_ptr covisibility_graph, - const Eigen::Vector3d& wall_pose, - const Eigen::Vector3d& wall_point, - const std::vector x_planes_msg, - const std::vector y_planes_msg, - std::unordered_map& x_vert_planes, - std::unordered_map& y_vert_planes); + void factor_wall( + const std::shared_ptr covisibility_graph, + const Eigen::Vector3d& wall_pose, + const Eigen::Vector3d& wall_point, + const std::vector x_planes_msg, + const std::vector y_planes_msg, + std::unordered_map& x_vert_planes, + std::unordered_map& y_vert_planes); /** * @brief diff --git a/s_graphs/include/s_graphs/common/plane_utils.hpp b/s_graphs/include/s_graphs/common/plane_utils.hpp index b33b4ca..35b016d 100644 --- a/s_graphs/include/s_graphs/common/plane_utils.hpp +++ b/s_graphs/include/s_graphs/common/plane_utils.hpp @@ -41,7 +41,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" namespace s_graphs { @@ -93,8 +93,9 @@ class PlaneUtils { * @param * @return */ - static float width_between_planes(const s_graphs_msgs::msg::PlaneData& plane1, - const s_graphs_msgs::msg::PlaneData& plane2); + static float width_between_planes( + const situational_graphs_msgs::msg::PlaneData& plane1, + const situational_graphs_msgs::msg::PlaneData& plane2); /** * @brief @@ -103,7 +104,7 @@ class PlaneUtils { * @return */ static void correct_plane_direction(int plane_type, - s_graphs_msgs::msg::PlaneData& plane); + situational_graphs_msgs::msg::PlaneData& plane); /** * @brief @@ -140,10 +141,10 @@ class PlaneUtils { * @return */ static geometry_msgs::msg::Pose room_center( - const s_graphs_msgs::msg::PlaneData& x_plane1, - const s_graphs_msgs::msg::PlaneData& x_plane2, - const s_graphs_msgs::msg::PlaneData& y_plane1, - const s_graphs_msgs::msg::PlaneData& y_plane2); + const situational_graphs_msgs::msg::PlaneData& x_plane1, + const situational_graphs_msgs::msg::PlaneData& x_plane2, + const situational_graphs_msgs::msg::PlaneData& y_plane1, + const situational_graphs_msgs::msg::PlaneData& y_plane2); /** * @brief @@ -208,8 +209,8 @@ class PlaneUtils { * @param * @return */ - static float plane_dot_product(const s_graphs_msgs::msg::PlaneData& plane1, - const s_graphs_msgs::msg::PlaneData& plane2); + static float plane_dot_product(const situational_graphs_msgs::msg::PlaneData& plane1, + const situational_graphs_msgs::msg::PlaneData& plane2); static bool plane_dot_product(g2o::VertexPlane* plane1, g2o::VertexPlane* plane2); @@ -217,8 +218,8 @@ class PlaneUtils { int plane_type, pcl::PointXY p1, pcl::PointXY p2, - s_graphs_msgs::msg::PlaneData plane1, - s_graphs_msgs::msg::PlaneData plane2, + situational_graphs_msgs::msg::PlaneData plane1, + situational_graphs_msgs::msg::PlaneData plane2, Eigen::Vector2d& cluster_center); /** * @brief diff --git a/s_graphs/include/s_graphs/common/room_utils.hpp b/s_graphs/include/s_graphs/common/room_utils.hpp index 7d8b6fb..e1893ed 100644 --- a/s_graphs/include/s_graphs/common/room_utils.hpp +++ b/s_graphs/include/s_graphs/common/room_utils.hpp @@ -64,17 +64,17 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include #include "geometry_msgs/msg/point.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "situational_graphs_reasoning_msgs/msg/attribute.hpp" #include "situational_graphs_reasoning_msgs/msg/edge.hpp" #include "situational_graphs_reasoning_msgs/msg/graph.hpp" #include "situational_graphs_reasoning_msgs/msg/graph_keyframes.hpp" #include "situational_graphs_reasoning_msgs/msg/node.hpp" #include "situational_graphs_reasoning_msgs/msg/room_keyframe.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" #include "visualization_msgs/msg/marker_array.hpp" struct PlaneGlobalRep { diff --git a/s_graphs/include/s_graphs/frontend/floor_analyzer.hpp b/s_graphs/include/s_graphs/frontend/floor_analyzer.hpp index c6066c6..7e83ddc 100644 --- a/s_graphs/include/s_graphs/frontend/floor_analyzer.hpp +++ b/s_graphs/include/s_graphs/frontend/floor_analyzer.hpp @@ -53,9 +53,9 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "tf2_ros/transform_listener.h" namespace s_graphs { @@ -84,9 +84,9 @@ class FloorAnalyzer { * @param floor_plane_candidates_vec */ void perform_floor_segmentation( - const std::vector& current_x_vert_planes, - const std::vector& current_y_vert_planes, - std::vector& floor_plane_candidates_vec); + const std::vector& current_x_vert_planes, + const std::vector& current_y_vert_planes, + std::vector& floor_plane_candidates_vec); }; } // namespace s_graphs diff --git a/s_graphs/include/s_graphs/frontend/room_analyzer.hpp b/s_graphs/include/s_graphs/frontend/room_analyzer.hpp index 1243e2a..a82c67f 100644 --- a/s_graphs/include/s_graphs/frontend/room_analyzer.hpp +++ b/s_graphs/include/s_graphs/frontend/room_analyzer.hpp @@ -47,9 +47,9 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include "geometry_msgs/msg/point.hpp" #include "rclcpp/rclcpp.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace s_graphs { @@ -60,17 +60,17 @@ struct room_analyzer_params { struct RoomInfo { public: - const std::vector& current_x_vert_planes; - const std::vector& current_y_vert_planes; + const std::vector& current_x_vert_planes; + const std::vector& current_y_vert_planes; pcl::PointCloud::Ptr cloud_cluster; }; struct RoomPlanes { public: - s_graphs_msgs::msg::PlaneData& x_plane1; - s_graphs_msgs::msg::PlaneData& x_plane2; - s_graphs_msgs::msg::PlaneData& y_plane1; - s_graphs_msgs::msg::PlaneData& y_plane2; + situational_graphs_msgs::msg::PlaneData& x_plane1; + situational_graphs_msgs::msg::PlaneData& x_plane2; + situational_graphs_msgs::msg::PlaneData& y_plane1; + situational_graphs_msgs::msg::PlaneData& y_plane2; bool found_x1_plane; bool found_x2_plane; bool found_y1_plane; @@ -213,8 +213,8 @@ class RoomAnalyzer { int plane_type, pcl::PointXY p1, pcl::PointXY p2, - s_graphs_msgs::msg::PlaneData plane1, - s_graphs_msgs::msg::PlaneData plane2, + situational_graphs_msgs::msg::PlaneData plane1, + situational_graphs_msgs::msg::PlaneData plane2, Eigen::Vector2d& cluster_center); /** @@ -231,7 +231,7 @@ class RoomAnalyzer { bool perform_room_segmentation( RoomInfo& room_info, pcl::PointCloud::Ptr cloud_cluster, - std::vector& room_candidates_vec, + std::vector& room_candidates_vec, const visualization_msgs::msg::MarkerArray& cloud_marker_array); /** @@ -303,9 +303,10 @@ class RoomAnalyzer { * @param plane * @return */ - std::vector find_plane_points(const pcl::PointXY& start_point, - const pcl::PointXY& end_point, - const s_graphs_msgs::msg::PlaneData& plane); + std::vector find_plane_points( + const pcl::PointXY& start_point, + const pcl::PointXY& end_point, + const situational_graphs_msgs::msg::PlaneData& plane); /** * @brief @@ -316,7 +317,7 @@ class RoomAnalyzer { * @return */ int find_plane_points(const pcl::PointCloud::Ptr& cloud_hull, - const s_graphs_msgs::msg::PlaneData& plane, + const situational_graphs_msgs::msg::PlaneData& plane, pcl::PointCloud::Ptr& sub_cloud_cluster); /** diff --git a/s_graphs/include/s_graphs/visualization/graph_publisher.hpp b/s_graphs/include/s_graphs/visualization/graph_publisher.hpp index f96aa2b..6198296 100644 --- a/s_graphs/include/s_graphs/visualization/graph_publisher.hpp +++ b/s_graphs/include/s_graphs/visualization/graph_publisher.hpp @@ -27,8 +27,6 @@ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE */ - - #ifndef GRAPH_PUBLISHER_HPP #define GRAPH_PUBLISHER_HPP @@ -79,20 +77,20 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include #include "geometry_msgs/msg/point.hpp" -#include "situational_graphs_reasoning_msgs/msg/attribute.hpp" -#include "situational_graphs_reasoning_msgs/msg/edge.hpp" -#include "situational_graphs_reasoning_msgs/msg/graph.hpp" -#include "situational_graphs_reasoning_msgs/msg/graph_keyframes.hpp" -#include "situational_graphs_reasoning_msgs/msg/node.hpp" #include "pcl_ros/transforms.hpp" #include "rclcpp/rclcpp.hpp" #include "s_graphs/common/room_utils.hpp" #include "s_graphs/common/ros_utils.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/planes_data.hpp" -#include "s_graphs_msgs/msg/room_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/planes_data.hpp" +#include "situational_graphs_msgs/msg/room_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_reasoning_msgs/msg/attribute.hpp" +#include "situational_graphs_reasoning_msgs/msg/edge.hpp" +#include "situational_graphs_reasoning_msgs/msg/graph.hpp" +#include "situational_graphs_reasoning_msgs/msg/graph_keyframes.hpp" +#include "situational_graphs_reasoning_msgs/msg/node.hpp" #include "visualization_msgs/msg/marker_array.hpp" class GraphPublisher { diff --git a/s_graphs/package.xml b/s_graphs/package.xml index daa0883..d056d00 100644 --- a/s_graphs/package.xml +++ b/s_graphs/package.xml @@ -31,7 +31,7 @@ backward_ros rviz_visual_tools libceres-dev - s_graphs_msgs + situational_graphs_msgs situational_graphs_reasoning situational_graphs_reasoning_msgs diff --git a/s_graphs/src/s_graphs/backend/finite_room_mapper.cpp b/s_graphs/src/s_graphs/backend/finite_room_mapper.cpp index c4fd821..bf8b9ab 100644 --- a/s_graphs/src/s_graphs/backend/finite_room_mapper.cpp +++ b/s_graphs/src/s_graphs/backend/finite_room_mapper.cpp @@ -26,7 +26,7 @@ FiniteRoomMapper::~FiniteRoomMapper() {} bool FiniteRoomMapper::lookup_rooms( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& x_vert_planes, const std::unordered_map& y_vert_planes, std::deque>& dupl_x_vert_planes, @@ -626,7 +626,7 @@ bool FiniteRoomMapper::check_room_ids( void FiniteRoomMapper::map_room_from_existing_infinite_rooms( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_x_infinite_room, const s_graphs::InfiniteRooms& matched_y_infinite_room, const Eigen::Isometry3d& room_center, @@ -685,7 +685,7 @@ void FiniteRoomMapper::map_room_from_existing_infinite_rooms( void FiniteRoomMapper::map_room_from_existing_x_infinite_room( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_x_infinite_room, const Eigen::Isometry3d& room_center, std::unordered_map& rooms_vec, @@ -752,7 +752,7 @@ void FiniteRoomMapper::map_room_from_existing_x_infinite_room( void FiniteRoomMapper::map_room_from_existing_y_infinite_room( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData& det_room_data, + const situational_graphs_msgs::msg::RoomData& det_room_data, const s_graphs::InfiniteRooms& matched_y_infinite_room, const Eigen::Isometry3d& room_center, std::unordered_map& rooms_vec, diff --git a/s_graphs/src/s_graphs/backend/floor_mapper.cpp b/s_graphs/src/s_graphs/backend/floor_mapper.cpp index f8c2b9e..24f0367 100644 --- a/s_graphs/src/s_graphs/backend/floor_mapper.cpp +++ b/s_graphs/src/s_graphs/backend/floor_mapper.cpp @@ -8,7 +8,7 @@ FloorMapper::~FloorMapper() {} void FloorMapper::lookup_floors( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, std::unordered_map& floors_vec, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, @@ -52,7 +52,7 @@ void FloorMapper::lookup_floors( void FloorMapper::factor_floor_node( std::shared_ptr& graph_slam, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, std::unordered_map& floors_vec, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, @@ -91,7 +91,7 @@ void FloorMapper::factor_floor_node( void FloorMapper::update_floor_node( std::shared_ptr& graph_slam, g2o::VertexFloor* floor_node, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& rooms_vec, const std::unordered_map& x_infinite_rooms, const std::unordered_map& y_infinite_rooms) { diff --git a/s_graphs/src/s_graphs/backend/infinite_room_mapper.cpp b/s_graphs/src/s_graphs/backend/infinite_room_mapper.cpp index 3245dbc..abccd67 100644 --- a/s_graphs/src/s_graphs/backend/infinite_room_mapper.cpp +++ b/s_graphs/src/s_graphs/backend/infinite_room_mapper.cpp @@ -30,7 +30,7 @@ InfiniteRoomMapper::~InfiniteRoomMapper() {} bool InfiniteRoomMapper::lookup_infinite_rooms( std::shared_ptr& graph_slam, const int& plane_type, - const s_graphs_msgs::msg::RoomData room_data, + const situational_graphs_msgs::msg::RoomData room_data, const std::unordered_map& x_vert_planes, const std::unordered_map& y_vert_planes, std::deque>& dupl_x_vert_planes, diff --git a/s_graphs/src/s_graphs/backend/wall_mapper.cpp b/s_graphs/src/s_graphs/backend/wall_mapper.cpp index 4148dd7..f4fe6e6 100644 --- a/s_graphs/src/s_graphs/backend/wall_mapper.cpp +++ b/s_graphs/src/s_graphs/backend/wall_mapper.cpp @@ -10,8 +10,8 @@ void WallMapper::factor_wall( const std::shared_ptr covisibility_graph, const Eigen::Vector3d& wall_pose, const Eigen::Vector3d& wall_point, - const std::vector x_planes_msg, - const std::vector y_planes_msg, + const std::vector x_planes_msg, + const std::vector y_planes_msg, std::unordered_map& x_vert_planes, std::unordered_map& y_vert_planes) { if (x_planes_msg.size() == 2) { diff --git a/s_graphs/src/s_graphs/common/plane_utils.cpp b/s_graphs/src/s_graphs/common/plane_utils.cpp index 9eea2db..c8bd6e2 100644 --- a/s_graphs/src/s_graphs/common/plane_utils.cpp +++ b/s_graphs/src/s_graphs/common/plane_utils.cpp @@ -20,8 +20,9 @@ float PlaneUtils::width_between_planes(Eigen::Vector4d v1, Eigen::Vector4d v2) { return size; } -float PlaneUtils::width_between_planes(const s_graphs_msgs::msg::PlaneData& plane1, - const s_graphs_msgs::msg::PlaneData& plane2) { +float PlaneUtils::width_between_planes( + const situational_graphs_msgs::msg::PlaneData& plane1, + const situational_graphs_msgs::msg::PlaneData& plane2) { Eigen::Vector3d vec; Eigen::Vector3d plane1_eigen, plane2_eigen; plane1_eigen << plane1.nx, plane1.ny, plane1.nz; @@ -38,8 +39,9 @@ float PlaneUtils::width_between_planes(const s_graphs_msgs::msg::PlaneData& plan return size; } -void PlaneUtils::correct_plane_direction(int plane_type, - s_graphs_msgs::msg::PlaneData& plane) { +void PlaneUtils::correct_plane_direction( + int plane_type, + situational_graphs_msgs::msg::PlaneData& plane) { if (plane.d > 0) { plane.nx = -1 * plane.nx; plane.ny = -1 * plane.ny; @@ -105,10 +107,10 @@ Eigen::Vector2d PlaneUtils::room_center(const Eigen::Vector4d& x_plane1, } geometry_msgs::msg::Pose PlaneUtils::room_center( - const s_graphs_msgs::msg::PlaneData& x_plane1, - const s_graphs_msgs::msg::PlaneData& x_plane2, - const s_graphs_msgs::msg::PlaneData& y_plane1, - const s_graphs_msgs::msg::PlaneData& y_plane2) { + const situational_graphs_msgs::msg::PlaneData& x_plane1, + const situational_graphs_msgs::msg::PlaneData& x_plane2, + const situational_graphs_msgs::msg::PlaneData& y_plane1, + const situational_graphs_msgs::msg::PlaneData& y_plane2) { geometry_msgs::msg::Pose center; Eigen::Vector3d vec_x, vec_y; Eigen::Vector3d x_plane1_eigen, x_plane2_eigen; @@ -269,8 +271,9 @@ bool PlaneUtils::compute_point_difference(const double plane1_point, return true; } -float PlaneUtils::plane_dot_product(const s_graphs_msgs::msg::PlaneData& plane1, - const s_graphs_msgs::msg::PlaneData& plane2) { +float PlaneUtils::plane_dot_product( + const situational_graphs_msgs::msg::PlaneData& plane1, + const situational_graphs_msgs::msg::PlaneData& plane2) { float dot_product = plane1.nx * plane2.nx + plane1.ny * plane2.ny + plane1.nz * plane2.nz; return dot_product; @@ -290,8 +293,8 @@ geometry_msgs::msg::Pose PlaneUtils::extract_infite_room_center( int plane_type, pcl::PointXY p1, pcl::PointXY p2, - s_graphs_msgs::msg::PlaneData plane1, - s_graphs_msgs::msg::PlaneData plane2, + situational_graphs_msgs::msg::PlaneData plane1, + situational_graphs_msgs::msg::PlaneData plane2, Eigen::Vector2d& cluster_center) { geometry_msgs::msg::Pose center_point; Eigen::Vector4d plane1_eigen, plane2_eigen; diff --git a/s_graphs/src/s_graphs/frontend/floor_analyzer.cpp b/s_graphs/src/s_graphs/frontend/floor_analyzer.cpp index 26a38b5..8044755 100644 --- a/s_graphs/src/s_graphs/frontend/floor_analyzer.cpp +++ b/s_graphs/src/s_graphs/frontend/floor_analyzer.cpp @@ -7,12 +7,12 @@ FloorAnalyzer::FloorAnalyzer() {} FloorAnalyzer::~FloorAnalyzer() {} void FloorAnalyzer::perform_floor_segmentation( - const std::vector& current_x_vert_planes, - const std::vector& current_y_vert_planes, - std::vector& floor_plane_candidates_vec) { + const std::vector& current_x_vert_planes, + const std::vector& current_y_vert_planes, + std::vector& floor_plane_candidates_vec) { // analyze the largest x plane pair - s_graphs_msgs::msg::PlaneData floor_x_plane1, floor_x_plane2; - s_graphs_msgs::msg::PlaneData floor_y_plane1, floor_y_plane2; + situational_graphs_msgs::msg::PlaneData floor_x_plane1, floor_x_plane2; + situational_graphs_msgs::msg::PlaneData floor_y_plane1, floor_y_plane2; floor_x_plane1.nx = -1; floor_x_plane2.nx = -1; floor_y_plane1.nx = -1; diff --git a/s_graphs/src/s_graphs/frontend/room_analyzer.cpp b/s_graphs/src/s_graphs/frontend/room_analyzer.cpp index bf8e417..c3cb21a 100644 --- a/s_graphs/src/s_graphs/frontend/room_analyzer.cpp +++ b/s_graphs/src/s_graphs/frontend/room_analyzer.cpp @@ -196,7 +196,7 @@ void RoomAnalyzer::extract_convex_hull( bool RoomAnalyzer::perform_room_segmentation( RoomInfo& room_info, pcl::PointCloud::Ptr cloud_cluster, - std::vector& room_candidates_vec, + std::vector& room_candidates_vec, const visualization_msgs::msg::MarkerArray& cloud_marker_array) { pcl::PointXY p1; pcl::PointXY p2; @@ -211,19 +211,19 @@ bool RoomAnalyzer::perform_room_segmentation( // if four planes are found its a bounded room // if 2 parallel planes are found it an ifnite corridor - s_graphs_msgs::msg::PlaneData x_plane1; + situational_graphs_msgs::msg::PlaneData x_plane1; x_plane1.nx = 0; x_plane1.ny = 0; x_plane1.nz = 0; - s_graphs_msgs::msg::PlaneData x_plane2; + situational_graphs_msgs::msg::PlaneData x_plane2; x_plane2.nx = 0; x_plane2.ny = 0; x_plane2.nz = 0; - s_graphs_msgs::msg::PlaneData y_plane1; + situational_graphs_msgs::msg::PlaneData y_plane1; y_plane1.nx = 0; y_plane1.ny = 0; y_plane1.nz = 0; - s_graphs_msgs::msg::PlaneData y_plane2; + situational_graphs_msgs::msg::PlaneData y_plane2; y_plane2.nx = 0; y_plane2.ny = 0; y_plane1.nz = 0; @@ -289,7 +289,7 @@ bool RoomAnalyzer::perform_room_segmentation( room_planes.y_plane1.plane_points.clear(); room_planes.y_plane2.plane_points.clear(); - s_graphs_msgs::msg::RoomData room_candidate; + situational_graphs_msgs::msg::RoomData room_candidate; room_candidate.id = cloud_cluster->header.seq; room_candidate.room_length = room_length; room_candidate.room_center = room_center; @@ -344,7 +344,7 @@ bool RoomAnalyzer::perform_room_segmentation( room_planes.y_plane1.plane_points.clear(); room_planes.y_plane2.plane_points.clear(); - s_graphs_msgs::msg::RoomData room_candidate; + situational_graphs_msgs::msg::RoomData room_candidate; room_candidate.id = cloud_cluster->header.seq; room_candidate.room_length = room_length; room_candidate.room_center = room_center; @@ -399,7 +399,7 @@ bool RoomAnalyzer::perform_room_segmentation( room_planes.y_plane1.plane_points.clear(); room_planes.y_plane2.plane_points.clear(); - s_graphs_msgs::msg::RoomData room_candidate; + situational_graphs_msgs::msg::RoomData room_candidate; room_candidate.id = cloud_cluster->header.seq; room_candidate.room_length = room_length; room_candidate.room_center = room_center; @@ -717,7 +717,7 @@ bool RoomAnalyzer::is_y2_plane_aligned_w_x( std::vector RoomAnalyzer::find_plane_points( const pcl::PointXY& start_point, const pcl::PointXY& end_point, - const s_graphs_msgs::msg::PlaneData& plane) { + const situational_graphs_msgs::msg::PlaneData& plane) { float min_start_point_plane_dist = 100; float min_end_point_plane_dist = 100; std::vector plane_point_distances; @@ -756,7 +756,7 @@ void RoomAnalyzer::downsample_cloud_data( int RoomAnalyzer::find_plane_points( const pcl::PointCloud::Ptr& cloud_hull, - const s_graphs_msgs::msg::PlaneData& plane, + const situational_graphs_msgs::msg::PlaneData& plane, pcl::PointCloud::Ptr& sub_cloud_cluster) { int num_neighbours = 0; double point_hull_dist_thres = 1.0; diff --git a/s_graphs/test/testRoom.cpp b/s_graphs/test/testRoom.cpp index 294ac08..fcbb58b 100644 --- a/s_graphs/test/testRoom.cpp +++ b/s_graphs/test/testRoom.cpp @@ -37,8 +37,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include #include "geometry_msgs/msg/point.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" typedef pcl::PointXYZI PointT; typedef pcl::PointXYZRGBNormal PointNormal; @@ -58,8 +58,8 @@ class TestRoom : public ::testing::Test { public: void testLookupRooms() { - s_graphs_msgs::msg::RoomData room_data; - s_graphs_msgs::msg::PlaneData x1_plane_data, x2_plane_data, y1_plane_data, + situational_graphs_msgs::msg::RoomData room_data; + situational_graphs_msgs::msg::PlaneData x1_plane_data, x2_plane_data, y1_plane_data, y2_plane_data; x1_plane_data.id = 1; x1_plane_data.nx = 1; @@ -183,7 +183,7 @@ class TestRoom : public ::testing::Test { dupl_x_vert_planes, dupl_y_vert_planes; std::unordered_map x_infinite_rooms, y_infinite_rooms; std::unordered_map rooms_vec; - s_graphs_msgs::msg::RoomData pub_room_data; + situational_graphs_msgs::msg::RoomData pub_room_data; }; TEST_F(TestRoom, TestRoomCreation) { diff --git a/s_graphs/test/testRoomCentreCompute.cpp b/s_graphs/test/testRoomCentreCompute.cpp index 61a32a1..8a519e3 100644 --- a/s_graphs/test/testRoomCentreCompute.cpp +++ b/s_graphs/test/testRoomCentreCompute.cpp @@ -38,8 +38,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE #include #include "geometry_msgs/msg/point.hpp" -#include "s_graphs_msgs/msg/plane_data.hpp" -#include "s_graphs_msgs/msg/rooms_data.hpp" +#include "situational_graphs_msgs/msg/plane_data.hpp" +#include "situational_graphs_msgs/msg/rooms_data.hpp" using namespace s_graphs; typedef pcl::PointXYZI PointT; typedef pcl::PointXYZRGBNormal PointNormal; @@ -60,8 +60,8 @@ class TestRoom : public ::testing::Test { public: void testLookupRooms() { - s_graphs_msgs::msg::RoomData room_data; - s_graphs_msgs::msg::PlaneData x1_plane_data, x2_plane_data, y1_plane_data, + situational_graphs_msgs::msg::RoomData room_data; + situational_graphs_msgs::msg::PlaneData x1_plane_data, x2_plane_data, y1_plane_data, y2_plane_data; x1_plane_data.id = 1; x1_plane_data.nx = 1; diff --git a/utils/msgs/s_graphs_msgs b/utils/msgs/s_graphs_msgs deleted file mode 160000 index eeddb2c..0000000 --- a/utils/msgs/s_graphs_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eeddb2c33579564ff821e3bad8321b90462b395b diff --git a/utils/msgs/situational_graphs_msgs b/utils/msgs/situational_graphs_msgs new file mode 160000 index 0000000..633f6e2 --- /dev/null +++ b/utils/msgs/situational_graphs_msgs @@ -0,0 +1 @@ +Subproject commit 633f6e24d72a658d8b43b87fc13f727d6dacd75d diff --git a/utils/reasoning/situational_graphs_reasoning b/utils/reasoning/situational_graphs_reasoning index 04024f4..c2a8ada 160000 --- a/utils/reasoning/situational_graphs_reasoning +++ b/utils/reasoning/situational_graphs_reasoning @@ -1 +1 @@ -Subproject commit 04024f42377de80c639917542b1454a9f21f89fe +Subproject commit c2a8ada8c42e5edde487679e0d3865f908a100c6