diff --git a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp index f5a52ad4a6032..0c6f770ac82a5 100644 --- a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp +++ b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp @@ -61,6 +61,7 @@ class ExternalCmdConverterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_emergency_stop_heartbeat_; + void takeData(); void onVelocity(const Odometry::ConstSharedPtr msg); void onExternalCmd(const ExternalControlCommand::ConstSharedPtr cmd_ptr); void onGearCommand(const GearCommand::ConstSharedPtr msg); diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index 32592c02c3347..277ae36886e77 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -26,17 +26,28 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; + auto noexec_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); pub_current_cmd_ = create_publisher("out/latest_external_control_cmd", rclcpp::QoS{1}); + + // subscriptions. sub_velocity_ = create_subscription( - "in/odometry", 1, std::bind(&ExternalCmdConverterNode::onVelocity, this, _1)); + "in/odometry", 1, std::bind(&ExternalCmdConverterNode::onVelocity, this, _1), noexec_subscription_options); + + // callback for external_control_cmd is executed in another thread. sub_control_cmd_ = create_subscription( "in/external_control_cmd", 1, std::bind(&ExternalCmdConverterNode::onExternalCmd, this, _1)); + sub_shift_cmd_ = create_subscription( - "in/shift_cmd", 1, std::bind(&ExternalCmdConverterNode::onGearCommand, this, _1)); + "in/shift_cmd", 1, std::bind(&ExternalCmdConverterNode::onGearCommand, this, _1), noexec_subscription_options); sub_gate_mode_ = create_subscription( - "in/current_gate_mode", 1, std::bind(&ExternalCmdConverterNode::onGateMode, this, _1)); + "in/current_gate_mode", 1, std::bind(&ExternalCmdConverterNode::onGateMode, this, _1), noexec_subscription_options); + + // callback for heartbeat_ is executed in another thread. sub_emergency_stop_heartbeat_ = create_subscription( "in/emergency_stop", 1, std::bind(&ExternalCmdConverterNode::onEmergencyStopHeartbeat, this, _1)); @@ -84,13 +95,32 @@ void ExternalCmdConverterNode::onTimer() updater_.force_update(); } +void ExternalCmdConverterNode::takeData() { + // take velocity + Odometry::SharedPtr velocity_msg = std::make_shared(); + GearCommand::SharedPtr shift_cmd_msg = std::make_shared(); + rclcpp::MessageInfo message_info; + + + if (sub_velocity_->take(*velocity_msg, message_info)) { + current_velocity_ptr_ = std::make_shared(velocity_msg->twist.twist.linear.x); + } + + if (sub_shift_cmd_->take(*shift_cmd_msg, message_info)) { + current_shift_cmd_ = shift_cmd_msg; + } + +} + void ExternalCmdConverterNode::onVelocity(const Odometry::ConstSharedPtr msg) { + assert(false); current_velocity_ptr_ = std::make_shared(msg->twist.twist.linear.x); } void ExternalCmdConverterNode::onGearCommand(const GearCommand::ConstSharedPtr msg) { + assert(false); current_shift_cmd_ = msg; } @@ -110,6 +140,8 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const pub_current_cmd_->publish(current_cmd); } + takeData(); + // Save received time for rate check latest_cmd_received_time_ = std::make_shared(this->now()); @@ -205,6 +237,7 @@ void ExternalCmdConverterNode::checkTopicStatus(diagnostic_updater::DiagnosticSt void ExternalCmdConverterNode::onGateMode( const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) { + assert(false); current_gate_mode_ = msg; } @@ -228,6 +261,14 @@ bool ExternalCmdConverterNode::checkEmergencyStopTopicTimeout() bool ExternalCmdConverterNode::checkRemoteTopicRate() { + tier4_control_msgs::msg::GateMode::SharedPtr current_gate_mode_msg = std::make_shared(); + + rclcpp::MessageInfo message_info; + + if (sub_gate_mode_->take(*current_gate_mode_msg, message_info)) { + current_gate_mode_ = current_gate_mode_msg; + } + if (!current_gate_mode_) { return true; }