Skip to content

Commit

Permalink
Fix stella_vslam_ros::System (Duplicated nodes)
Browse files Browse the repository at this point in the history
  • Loading branch information
@ymd-stella
ymd-stella committed Sep 3, 2023
1 parent 9d64a61 commit 4c2da9b
Show file tree
Hide file tree
Showing 5 changed files with 25 additions and 16 deletions.
9 changes: 5 additions & 4 deletions src/run_slam.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ void tracking(const std::shared_ptr<stella_vslam_ros::system>& slam_ros,
});
}

slam_ros->exec_.spin();
rclcpp::spin(slam_ros->node_->get_node_base_interface());

// automatically close the viewer
if (viewer_string == "pangolin_viewer") {
Expand Down Expand Up @@ -256,16 +256,17 @@ int main(int argc, char* argv[]) {
slam->disable_loop_detector();
}

auto node = std::make_shared<rclcpp::Node>("run_slam");
std::shared_ptr<stella_vslam_ros::system> slam_ros;
if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Monocular) {
slam_ros = std::make_shared<stella_vslam_ros::mono>(slam, mask_img_path->value());
slam_ros = std::make_shared<stella_vslam_ros::mono>(slam, node.get(), mask_img_path->value());
}
else if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Stereo) {
auto rectifier = rectify->value() ? std::make_shared<stella_vslam::util::stereo_rectifier>(cfg, slam->get_camera()) : nullptr;
slam_ros = std::make_shared<stella_vslam_ros::stereo>(slam, mask_img_path->value(), rectifier);
slam_ros = std::make_shared<stella_vslam_ros::stereo>(slam, node.get(), mask_img_path->value(), rectifier);
}
else if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::RGBD) {
slam_ros = std::make_shared<stella_vslam_ros::rgbd>(slam, mask_img_path->value());
slam_ros = std::make_shared<stella_vslam_ros::rgbd>(slam, node.get(), mask_img_path->value());
}
else {
throw std::runtime_error("Invalid setup type: " + slam->get_camera()->get_setup_type_string());
Expand Down
7 changes: 4 additions & 3 deletions src/run_slam_offline.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -402,16 +402,17 @@ int main(int argc, char* argv[]) {
slam->disable_loop_detector();
}

auto node = std::make_shared<rclcpp::Node>("run_slam");
std::shared_ptr<stella_vslam_ros::system> slam_ros;
if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Monocular) {
slam_ros = std::make_shared<stella_vslam_ros::mono>(slam, mask_img_path->value());
slam_ros = std::make_shared<stella_vslam_ros::mono>(slam, node.get(), mask_img_path->value());
}
else if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Stereo) {
auto rectifier = rectify->value() ? std::make_shared<stella_vslam::util::stereo_rectifier>(cfg, slam->get_camera()) : nullptr;
slam_ros = std::make_shared<stella_vslam_ros::stereo>(slam, mask_img_path->value(), rectifier);
slam_ros = std::make_shared<stella_vslam_ros::stereo>(slam, node.get(), mask_img_path->value(), rectifier);
}
else if (slam->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::RGBD) {
slam_ros = std::make_shared<stella_vslam_ros::rgbd>(slam, mask_img_path->value());
slam_ros = std::make_shared<stella_vslam_ros::rgbd>(slam, node.get(), mask_img_path->value());
}
else {
throw std::runtime_error("Invalid setup type: " + slam->get_camera()->get_setup_type_string());
Expand Down
16 changes: 10 additions & 6 deletions src/stella_vslam_ros.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,9 @@ Eigen::Affine3d project_to_xy_plane(const Eigen::Affine3d& affine) {

namespace stella_vslam_ros {
system::system(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path)
: slam_(slam), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_sensor_data),
: slam_(slam), node_(node), custom_qos_(rmw_qos_profile_sensor_data),
mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)),
keyframes_pub_(node_->create_publisher<geometry_msgs::msg::PoseArray>("~/keyframes", 1)),
Expand All @@ -36,7 +37,6 @@ system::system(const std::shared_ptr<stella_vslam::system>& slam,
tf_(std::make_unique<tf2_ros::Buffer>(node_->get_clock())),
transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
custom_qos_.depth = 1;
exec_.add_node(node_);
init_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"/initialpose", 1,
std::bind(&system::init_pose_callback,
Expand Down Expand Up @@ -210,8 +210,9 @@ void system::init_pose_callback(
}

mono::mono(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path)
: system(slam, mask_img_path) {
: system(slam, node, mask_img_path) {
auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_), custom_qos_);
raw_image_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
"camera/image_raw", qos, [this](sensor_msgs::msg::Image::UniquePtr msg_unique_ptr) { callback(std::move(msg_unique_ptr)); });
Expand Down Expand Up @@ -266,9 +267,10 @@ void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
}

stereo::stereo(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path,
const std::shared_ptr<stella_vslam::util::stereo_rectifier>& rectifier)
: system(slam, mask_img_path),
: system(slam, node, mask_img_path),
rectifier_(rectifier),
left_sf_(node_, "camera/left/image_raw"),
right_sf_(node_, "camera/right/image_raw") {
Expand Down Expand Up @@ -318,8 +320,10 @@ void stereo::callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const
}
}

rgbd::rgbd(const std::shared_ptr<stella_vslam::system>& slam, const std::string& mask_img_path)
: system(slam, mask_img_path),
rgbd::rgbd(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path)
: system(slam, node, mask_img_path),
color_sf_(node_, "camera/color/image_raw"),
depth_sf_(node_, "camera/depth/image_raw") {
use_exact_time_ = false;
Expand Down
7 changes: 5 additions & 2 deletions src/stella_vslam_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,14 @@ namespace stella_vslam_ros {
class system {
public:
system(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path);
void publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp);
void publish_keyframes(const rclcpp::Time& stamp);
void setParams();
std::shared_ptr<stella_vslam::system> slam_;
std::shared_ptr<stella_vslam::config> cfg_;
std::shared_ptr<rclcpp::Node> node_;
rclcpp::executors::SingleThreadedExecutor exec_;
rclcpp::Node* node_;
rmw_qos_profile_t custom_qos_;
cv::Mat mask_;
std::vector<double> track_times_;
Expand Down Expand Up @@ -74,6 +74,7 @@ class system {
class mono : public system {
public:
mono(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path);
void callback(sensor_msgs::msg::Image::UniquePtr msg);
void callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg);
Expand Down Expand Up @@ -112,6 +113,7 @@ class ModifiedSubscriber : public message_filters::Subscriber<M> {
class stereo : public system {
public:
stereo(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path,
const std::shared_ptr<stella_vslam::util::stereo_rectifier>& rectifier);
void callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const sensor_msgs::msg::Image::ConstSharedPtr& right);
Expand All @@ -128,6 +130,7 @@ class stereo : public system {
class rgbd : public system {
public:
rgbd(const std::shared_ptr<stella_vslam::system>& slam,
rclcpp::Node* node,
const std::string& mask_img_path);
void callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, const sensor_msgs::msg::Image::ConstSharedPtr& depth);

Expand Down
2 changes: 1 addition & 1 deletion src/system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ System::System(
}

if (slam_->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Monocular) {
slam_ros_ = std::make_shared<stella_vslam_ros::mono>(slam_, "");
slam_ros_ = std::make_shared<stella_vslam_ros::mono>(slam_, this, "");
}
else {
RCLCPP_FATAL_STREAM(get_logger(), "Invalid setup type: " << slam_->get_camera()->get_setup_type_string());
Expand Down

0 comments on commit 4c2da9b

Please sign in to comment.