Skip to content

Commit

Permalink
fix: container die problem
Browse files Browse the repository at this point in the history
Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
Berkay Karaman committed May 17, 2024
1 parent d459e4a commit 08833c6
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 3 deletions.
1 change: 1 addition & 0 deletions tools/reaction_analyzer/include/reaction_analyzer_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
std::optional<rclcpp::Time> test_environment_init_time_;
std::optional<rclcpp::Time> spawn_cmd_time_;
std::atomic<bool> spawn_object_cmd_{false};
std::atomic<bool> ego_initialized_{false};
bool is_initialization_requested{false};
bool is_route_set_{false};
size_t test_iteration_count_{0};
Expand Down
4 changes: 3 additions & 1 deletion tools/reaction_analyzer/include/topic_publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,7 @@ class TopicPublisher
{
public:
explicit TopicPublisher(
rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd,
rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd, std::atomic<bool> & ego_initialized,
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
const EntityParams & entity_params);

Expand All @@ -192,6 +192,8 @@ class TopicPublisher
rclcpp::Node * node_;
RunningMode node_running_mode_;
std::atomic<bool> & spawn_object_cmd_;
std::atomic<bool> & ego_initialized_; // used for planning_control mode because if pointcloud is
// published before ego is initialized, it causes crash
EntityParams entity_params_;
std::optional<rclcpp::Time> & spawn_cmd_time_; // Set by a publisher function when the
// spawn_object_cmd_ is true
Expand Down
5 changes: 4 additions & 1 deletion tools/reaction_analyzer/src/reaction_analyzer_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,8 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
}

topic_publisher_ptr_ = std::make_unique<topic_publisher::TopicPublisher>(
this, spawn_object_cmd_, spawn_cmd_time_, node_running_mode_, node_params_.entity_params);
this, spawn_object_cmd_, ego_initialized_, spawn_cmd_time_, node_running_mode_,
node_params_.entity_params);

// initialize the odometry before init the subscriber
odometry_ptr_ = std::make_shared<Odometry>();
Expand Down Expand Up @@ -299,6 +300,7 @@ void ReactionAnalyzerNode::init_test_env(
call_operation_mode_service_without_response();
}
}
ego_initialized_ = true;

const bool is_ready =
(is_initialization_requested && is_route_set_ &&
Expand Down Expand Up @@ -419,6 +421,7 @@ void ReactionAnalyzerNode::reset()
std::lock_guard<std::mutex> lock(mutex_);
spawn_cmd_time_ = std::nullopt;
subscriber_ptr_->reset();
ego_initialized_ = false;
RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_);
}
} // namespace reaction_analyzer
Expand Down
6 changes: 5 additions & 1 deletion tools/reaction_analyzer/src/topic_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,13 @@ namespace reaction_analyzer::topic_publisher
{

TopicPublisher::TopicPublisher(
rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd,
rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd, std::atomic<bool> & ego_initialized,
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
const EntityParams & entity_params)
: node_(node),
node_running_mode_(node_running_mode),
spawn_object_cmd_(spawn_object_cmd),
ego_initialized_(ego_initialized),
entity_params_(entity_params),
spawn_cmd_time_(spawn_cmd_time)
{
Expand Down Expand Up @@ -192,6 +193,9 @@ void TopicPublisher::generic_message_publisher(const std::string & topic_name)

void TopicPublisher::dummy_perception_publisher()
{
if (!ego_initialized_) {
return; // do not publish anything if ego is not initialized
}
if (!spawn_object_cmd_) {
// do not spawn it, send empty pointcloud
pcl::PointCloud<pcl::PointXYZ> pcl_empty;
Expand Down

0 comments on commit 08833c6

Please sign in to comment.