Skip to content

Commit

Permalink
[Feat] Change s_graphs_msgs->situational_graphs_msgs
Browse files Browse the repository at this point in the history
  • Loading branch information
@hridaybavle
hridaybavle committed May 13, 2024
1 parent e2adb14 commit 888f806
Show file tree
Hide file tree
Showing 29 changed files with 295 additions and 260 deletions.
8 changes: 4 additions & 4 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
4 changes: 2 additions & 2 deletions .rosinstall_ros2
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
4 changes: 2 additions & 2 deletions s_graphs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -99,7 +99,7 @@ set(DEPENDENCIES
nmea_msgs
geodesy
rviz_visual_tools
s_graphs_msgs)
situational_graphs_msgs)

SET(G2O_COMMON_LIBS
${PCL_LIBRARIES}
Expand Down
65 changes: 36 additions & 29 deletions s_graphs/apps/floor_plan_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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_msgs::msg::PlanesData>(
"s_graphs/all_map_planes",
100,
std::bind(&FloorPlanNode::map_planes_callback, this, std::placeholders::_1));

all_rooms_data_pub = this->create_publisher<s_graphs_msgs::msg::RoomsData>(
"floor_plan/all_rooms_data", 1);
floor_data_pub = this->create_publisher<s_graphs_msgs::msg::RoomData>(
map_planes_sub =
this->create_subscription<situational_graphs_msgs::msg::PlanesData>(
"s_graphs/all_map_planes",
100,
std::bind(
&FloorPlanNode::map_planes_callback, this, std::placeholders::_1));

all_rooms_data_pub =
this->create_publisher<situational_graphs_msgs::msg::RoomsData>(
"floor_plan/all_rooms_data", 1);
floor_data_pub = this->create_publisher<situational_graphs_msgs::msg::RoomData>(
"floor_plan/floor_data", 1);

floor_plane_timer = create_wall_timer(
Expand Down Expand Up @@ -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<std::mutex> 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<s_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
std::vector<s_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::vector<situational_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
std::vector<situational_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::lock_guard<std::mutex> 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) {
Expand Down Expand Up @@ -158,7 +161,7 @@ class FloorPlanNode : public rclcpp::Node {
}

void floor_plan_callback() {
std::vector<s_graphs_msgs::msg::PlaneData> current_x_vert_planes,
std::vector<situational_graphs_msgs::msg::PlaneData> current_x_vert_planes,
current_y_vert_planes;
flush_map_planes(current_x_vert_planes, current_y_vert_planes);

Expand All @@ -182,9 +185,10 @@ class FloorPlanNode : public rclcpp::Node {
}

void extract_rooms(
const std::vector<s_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
const std::vector<s_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::vector<s_graphs_msgs::msg::RoomData> room_candidates_vec;
const std::vector<situational_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
const std::vector<situational_graphs_msgs::msg::PlaneData>&
current_y_vert_planes) {
std::vector<situational_graphs_msgs::msg::RoomData> room_candidates_vec;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> curr_cloud_clusters =
room_analyzer->extract_cloud_clusters();

Expand All @@ -207,17 +211,18 @@ 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);
}
}

void extract_floors(
const std::vector<s_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
const std::vector<s_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::vector<s_graphs_msgs::msg::PlaneData> floor_plane_candidates_vec;
const std::vector<situational_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
const std::vector<situational_graphs_msgs::msg::PlaneData>&
current_y_vert_planes) {
std::vector<situational_graphs_msgs::msg::PlaneData> floor_plane_candidates_vec;
floor_analyzer->perform_floor_segmentation(
current_x_vert_planes, current_y_vert_planes, floor_plane_candidates_vec);

Expand All @@ -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]);
Expand All @@ -243,9 +248,11 @@ class FloorPlanNode : public rclcpp::Node {
private:
rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr
skeleton_graph_sub;
rclcpp::Subscription<s_graphs_msgs::msg::PlanesData>::SharedPtr map_planes_sub;
rclcpp::Publisher<s_graphs_msgs::msg::RoomsData>::SharedPtr all_rooms_data_pub;
rclcpp::Publisher<s_graphs_msgs::msg::RoomData>::SharedPtr floor_data_pub;
rclcpp::Subscription<situational_graphs_msgs::msg::PlanesData>::SharedPtr
map_planes_sub;
rclcpp::Publisher<situational_graphs_msgs::msg::RoomsData>::SharedPtr
all_rooms_data_pub;
rclcpp::Publisher<situational_graphs_msgs::msg::RoomData>::SharedPtr floor_data_pub;

private:
rclcpp::TimerBase::SharedPtr floor_plane_timer;
Expand All @@ -257,7 +264,7 @@ class FloorPlanNode : public rclcpp::Node {
std::unique_ptr<FloorAnalyzer> floor_analyzer;

std::mutex map_plane_mutex;
std::deque<std::vector<s_graphs_msgs::msg::PlaneData>> x_vert_plane_queue,
std::deque<std::vector<situational_graphs_msgs::msg::PlaneData>> x_vert_plane_queue,
y_vert_plane_queue;
};

Expand Down
45 changes: 25 additions & 20 deletions s_graphs/apps/room_segmentation_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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_msgs::msg::PlanesData>(
"s_graphs/map_planes",
100,
std::bind(
&RoomSegmentationNode::map_planes_callback, this, std::placeholders::_1));
map_planes_sub =
this->create_subscription<situational_graphs_msgs::msg::PlanesData>(
"s_graphs/map_planes",
100,
std::bind(&RoomSegmentationNode::map_planes_callback,
this,
std::placeholders::_1));

cluster_cloud_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>(
"room_segmentation/cluster_cloud", 1);
cluster_clouds_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>(
"room_segmentation/cluster_clouds", 1);
room_data_pub = this->create_publisher<s_graphs_msgs::msg::RoomsData>(
room_data_pub = this->create_publisher<situational_graphs_msgs::msg::RoomsData>(
"room_segmentation/room_data", 1);
room_centers_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>(
"room_segmentation/room_centers", 1);
Expand All @@ -138,7 +140,7 @@ class RoomSegmentationNode : public rclcpp::Node {
}

void room_detection_callback() {
std::vector<s_graphs_msgs::msg::PlaneData> current_x_vert_planes,
std::vector<situational_graphs_msgs::msg::PlaneData> current_x_vert_planes,
current_y_vert_planes;
flush_map_planes(current_x_vert_planes, current_y_vert_planes);

Expand All @@ -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<std::mutex> 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<s_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
std::vector<s_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::vector<situational_graphs_msgs::msg::PlaneData>& current_x_vert_planes,
std::vector<situational_graphs_msgs::msg::PlaneData>& current_y_vert_planes) {
std::lock_guard<std::mutex> 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) {
Expand Down Expand Up @@ -215,11 +217,12 @@ class RoomSegmentationNode : public rclcpp::Node {
* @brief extract clusters with its centers from the skeletal cloud
*
*/
void extract_rooms(std::vector<s_graphs_msgs::msg::PlaneData> current_x_vert_planes,
std::vector<s_graphs_msgs::msg::PlaneData> current_y_vert_planes) {
void extract_rooms(
std::vector<situational_graphs_msgs::msg::PlaneData> current_x_vert_planes,
std::vector<situational_graphs_msgs::msg::PlaneData> current_y_vert_planes) {
int room_cluster_counter = 0;
visualization_msgs::msg::MarkerArray refined_skeleton_marker_array;
std::vector<s_graphs_msgs::msg::RoomData> room_candidates_vec;
std::vector<situational_graphs_msgs::msg::RoomData> room_candidates_vec;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_visualizer(
new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull_visualizer(
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -327,20 +330,22 @@ class RoomSegmentationNode : public rclcpp::Node {
private:
rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr
skeleton_graph_sub;
rclcpp::Subscription<s_graphs_msgs::msg::PlanesData>::SharedPtr map_planes_sub;
rclcpp::Subscription<situational_graphs_msgs::msg::PlanesData>::SharedPtr
map_planes_sub;

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_cloud_pub;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_clouds_pub;
rclcpp::Publisher<s_graphs_msgs::msg::RoomsData>::SharedPtr room_data_pub;
rclcpp::Publisher<situational_graphs_msgs::msg::RoomsData>::SharedPtr room_data_pub;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr room_centers_pub;

/* private variables */
private:
rclcpp::TimerBase::SharedPtr room_detection_timer;
std::mutex map_plane_mutex;

std::vector<s_graphs_msgs::msg::PlaneData> x_vert_plane_vec, y_vert_plane_vec;
std::deque<std::vector<s_graphs_msgs::msg::PlaneData>> x_vert_plane_queue,
std::vector<situational_graphs_msgs::msg::PlaneData> x_vert_plane_vec,
y_vert_plane_vec;
std::deque<std::vector<situational_graphs_msgs::msg::PlaneData>> x_vert_plane_queue,
y_vert_plane_queue;

std::unique_ptr<RoomAnalyzer> room_analyzer;
Expand Down
Loading

0 comments on commit 888f806

Please sign in to comment.