diff --git a/ardusub_manager/src/ardusub_manager.cpp b/ardusub_manager/src/ardusub_manager.cpp index fd1deb1..cb86e5d 100644 --- a/ardusub_manager/src/ardusub_manager.cpp +++ b/ardusub_manager/src/ardusub_manager.cpp @@ -23,6 +23,7 @@ #include #include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/qos.hpp" namespace ardusub_manager { @@ -68,156 +69,156 @@ ArduSubManager::ArduSubManager() // NOLINT void ArduSubManager::set_message_rate(int64_t msg_id, double rate) const { - // auto request = std::make_shared(); - // request->message_id = msg_id; - // request->message_rate = rate; + auto request = std::make_shared(); + request->message_id = msg_id; + request->message_rate = rate; - // using namespace std::chrono_literals; + using namespace std::chrono_literals; - // auto future_result = set_message_intervals_client_->async_send_request(std::move(request)).future.share(); - // auto future_status = wait_for_result(future_result, 5s); + auto future_result = set_message_intervals_client_->async_send_request(std::move(request)).future.share(); + auto future_status = wait_for_result(future_result, 5s); - // if (future_status != std::future_status::ready) { - // RCLCPP_ERROR( // NOLINT - // this->get_logger(), "A timeout occurred while attempting to set the message interval for message ID %ld", msg_id); - // } + if (future_status != std::future_status::ready) { + RCLCPP_ERROR( // NOLINT + this->get_logger(), "A timeout occurred while attempting to set the message interval for message ID %ld", msg_id); + } - // if (!future_result.get()->success) { - // RCLCPP_ERROR(this->get_logger(), "Failed to set the message interval for message ID %ld", msg_id); // NOLINT - // } + if (!future_result.get()->success) { + RCLCPP_ERROR(this->get_logger(), "Failed to set the message interval for message ID %ld", msg_id); // NOLINT + } - // RCLCPP_DEBUG(this->get_logger(), "Message %ld set to publish at a rate of %f hz", msg_id, rate); // NOLINT + RCLCPP_DEBUG(this->get_logger(), "Message %ld set to publish at a rate of %f hz", msg_id, rate); // NOLINT } void ArduSubManager::set_message_rates(const std::vector & msg_ids, const std::vector & rates) const { - // for (size_t i = 0; i < msg_ids.size(); ++i) { - // set_message_rate(msg_ids[i], rates[i]); - // } + for (size_t i = 0; i < msg_ids.size(); ++i) { + set_message_rate(msg_ids[i], rates[i]); + } } CallbackReturn ArduSubManager::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO(this->get_logger(), "Configuring the ArduSub manager..."); // NOLINT - - // try { - // param_listener_ = std::make_shared(this->get_node_parameters_interface()); - // params_ = param_listener_->get_params(); - // } - // catch (const std::exception & e) { - // RCLCPP_ERROR( // NOLINT - // this->get_logger(), "An exception occurred while initializing the ArduSub manager: %s\n", e.what()); - // return CallbackReturn::ERROR; - // } - - // set_ekf_origin_ = params_.set_ekf_origin; - - // using namespace std::chrono_literals; - - // // Use a reentrant callback group so that we can call the service from within the on_activate/deactivate functions - // set_intervals_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - // if (!params_.message_intervals.ids.empty()) { - // if (params_.message_intervals.rates.size() != params_.message_intervals.ids.size()) { - // fprintf(stderr, "The number of message IDs does not match the number of message rates\n"); - // return CallbackReturn::ERROR; - // } - - // set_message_intervals_client_ = this->create_client( - // "/mavros/set_message_interval", rclcpp::SystemDefaultsQoS(), set_intervals_callback_group_); - - // set_intervals_timer_ = this->create_wall_timer( - // 20s, [this]() -> void { set_message_rates(params_.message_intervals.ids, params_.message_intervals.rates); }); - - // // Wait to start the timer until the manager has been activated - // set_intervals_timer_->cancel(); - // } - - // if (set_ekf_origin_) { - // // Setup the publisher for the EKF origin - // auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); - // ekf_origin_pub_ = - // this->create_publisher("/mavros/global_position/set_gp_origin", qos); - - // // Periodically publish the EKF origin - // set_ekf_origin_timer_ = this->create_wall_timer(20s, [this]() -> void { - // RCLCPP_DEBUG(this->get_logger(), "Setting the EKF origin"); // NOLINT - // geographic_msgs::msg::GeoPointStamped ekf_origin; - // ekf_origin.header.stamp = this->get_clock()->now(); - // ekf_origin.position.latitude = params_.ekf_origin.latitude; - // ekf_origin.position.longitude = params_.ekf_origin.longitude; - // ekf_origin.position.altitude = params_.ekf_origin.altitude; - - // ekf_origin_pub_->publish(ekf_origin); - // }); - - // // Wait to start the timer until the manager has been activated - // set_ekf_origin_timer_->cancel(); - // } - - // // Publish the system TF - // if (params_.publish_tf) { - // tf_broadcaster_ = std::make_unique(*this); - - // pose_sub_ = this->create_subscription( - // "/mavros/local_position/pose", rclcpp::SensorDataQoS(), - // [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { // NOLINT - // geometry_msgs::msg::TransformStamped transform; - - // transform.header.stamp = msg->header.stamp; - // transform.header.frame_id = "map"; - // transform.child_frame_id = "base_link"; - - // transform.transform.translation.x = msg->pose.position.x; - // transform.transform.translation.y = msg->pose.position.y; - // transform.transform.translation.z = msg->pose.position.z; - // transform.transform.rotation = msg->pose.orientation; - - // tf_broadcaster_->sendTransform(transform); - // }); - // } - - // RCLCPP_INFO(this->get_logger(), "Successfully configured the ArduSub manager!"); // NOLINT + RCLCPP_INFO(this->get_logger(), "Configuring the ArduSub manager..."); // NOLINT + + try { + param_listener_ = std::make_shared(this->get_node_parameters_interface()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) { + RCLCPP_ERROR( // NOLINT + this->get_logger(), "An exception occurred while initializing the ArduSub manager: %s\n", e.what()); + return CallbackReturn::ERROR; + } + + set_ekf_origin_ = params_.set_ekf_origin; + + using namespace std::chrono_literals; + + // Use a reentrant callback group so that we can call the service from within the on_activate/deactivate functions + set_intervals_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + if (!params_.message_intervals.ids.empty()) { + if (params_.message_intervals.rates.size() != params_.message_intervals.ids.size()) { + fprintf(stderr, "The number of message IDs does not match the number of message rates\n"); + return CallbackReturn::ERROR; + } + + set_message_intervals_client_ = this->create_client( + "/mavros/set_message_interval", rmw_qos_profile_system_default, set_intervals_callback_group_); + + set_intervals_timer_ = this->create_wall_timer( + 20s, [this]() -> void { set_message_rates(params_.message_intervals.ids, params_.message_intervals.rates); }); + + // Wait to start the timer until the manager has been activated + set_intervals_timer_->cancel(); + } + + if (set_ekf_origin_) { + // Setup the publisher for the EKF origin + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + ekf_origin_pub_ = + this->create_publisher("/mavros/global_position/set_gp_origin", qos); + + // Periodically publish the EKF origin + set_ekf_origin_timer_ = this->create_wall_timer(20s, [this]() -> void { + RCLCPP_DEBUG(this->get_logger(), "Setting the EKF origin"); // NOLINT + geographic_msgs::msg::GeoPointStamped ekf_origin; + ekf_origin.header.stamp = this->get_clock()->now(); + ekf_origin.position.latitude = params_.ekf_origin.latitude; + ekf_origin.position.longitude = params_.ekf_origin.longitude; + ekf_origin.position.altitude = params_.ekf_origin.altitude; + + ekf_origin_pub_->publish(ekf_origin); + }); + + // Wait to start the timer until the manager has been activated + set_ekf_origin_timer_->cancel(); + } + + // Publish the system TF + if (params_.publish_tf) { + tf_broadcaster_ = std::make_unique(*this); + + pose_sub_ = this->create_subscription( + "/mavros/local_position/pose", rclcpp::SensorDataQoS(), + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { // NOLINT + geometry_msgs::msg::TransformStamped transform; + + transform.header.stamp = msg->header.stamp; + transform.header.frame_id = "map"; + transform.child_frame_id = "base_link"; + + transform.transform.translation.x = msg->pose.position.x; + transform.transform.translation.y = msg->pose.position.y; + transform.transform.translation.z = msg->pose.position.z; + transform.transform.rotation = msg->pose.orientation; + + tf_broadcaster_->sendTransform(transform); + }); + } + + RCLCPP_INFO(this->get_logger(), "Successfully configured the ArduSub manager!"); // NOLINT return CallbackReturn::SUCCESS; } CallbackReturn ArduSubManager::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO(this->get_logger(), "Activating the ArduSub manager..."); // NOLINT + RCLCPP_INFO(this->get_logger(), "Activating the ArduSub manager..."); // NOLINT - // using namespace std::chrono_literals; - - // // Set the EKF origin - // if (set_ekf_origin_) { - // set_ekf_origin_timer_->reset(); - // } + using namespace std::chrono_literals; - // // Set the message intervals - // if (!params_.message_intervals.ids.empty()) { - // while (!set_message_intervals_client_->wait_for_service(1s)) { - // if (!rclcpp::ok()) { - // RCLCPP_INFO( // NOLINT - // this->get_logger(), "Interrupted while waiting for the message interval service to exist"); - // return CallbackReturn::ERROR; - // } - // RCLCPP_INFO(this->get_logger(), "Waiting for the message interval service to exist..."); // NOLINT - // } + // Set the EKF origin + if (set_ekf_origin_) { + set_ekf_origin_timer_->reset(); + } + + // Set the message intervals + if (!params_.message_intervals.ids.empty()) { + while (!set_message_intervals_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_INFO( // NOLINT + this->get_logger(), "Interrupted while waiting for the message interval service to exist"); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(this->get_logger(), "Waiting for the message interval service to exist..."); // NOLINT + } - // set_intervals_timer_->reset(); - // } + set_intervals_timer_->reset(); + } - // RCLCPP_INFO(this->get_logger(), "Successfully activated the ArduSub manager!"); // NOLINT + RCLCPP_INFO(this->get_logger(), "Successfully activated the ArduSub manager!"); // NOLINT return CallbackReturn::SUCCESS; } CallbackReturn ArduSubManager::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO(this->get_logger(), "Deactivating the ArduSub manager..."); // NOLINT + RCLCPP_INFO(this->get_logger(), "Deactivating the ArduSub manager..."); // NOLINT - // set_intervals_timer_->cancel(); - // set_ekf_origin_timer_->cancel(); + set_intervals_timer_->cancel(); + set_ekf_origin_timer_->cancel(); return CallbackReturn::SUCCESS; } @@ -236,16 +237,16 @@ CallbackReturn ArduSubManager::on_shutdown(const rclcpp_lifecycle::State & /*pre int main(int argc, char * argv[]) { - // rclcpp::init(argc, argv); + rclcpp::init(argc, argv); - // rclcpp::executors::MultiThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; - // auto node = std::make_shared(); - // executor.add_node(node->get_node_base_interface()); + auto node = std::make_shared(); + executor.add_node(node->get_node_base_interface()); - // executor.spin(); + executor.spin(); - // rclcpp::shutdown(); + rclcpp::shutdown(); return 0; } diff --git a/ardusub_teleop/src/joy_interface.cpp b/ardusub_teleop/src/joy_interface.cpp index 3a533a7..6d88c87 100644 --- a/ardusub_teleop/src/joy_interface.cpp +++ b/ardusub_teleop/src/joy_interface.cpp @@ -36,53 +36,53 @@ int scale_cmd(float value, int old_min, int old_max, int new_min, int new_max) JoyInterface::JoyInterface() : Node("joy_interface") { - // this->declare_parameter("min_pwm", 1100); - // this->declare_parameter("max_pwm", 1900); - - // pwm_range_ = std::make_tuple(this->get_parameter("min_pwm").as_int(), this->get_parameter("max_pwm").as_int()); - - // enable_pwm_service_ = create_service( - // "~/enable_pwm_control", [this]( - // const std::shared_ptr request, // NOLINT - // std::shared_ptr response) { // NOLINT - // pwm_enabled_ = request->data; - // response->success = true; - // return; - // }); - - // const auto qos_profile = rclcpp::SystemDefaultsQoS(); - // velocity_pub_ = create_publisher("/cmd_vel", qos_profile); - // rc_override_pub_ = create_publisher("/mavros/rc/override", qos_profile); - - // cmd_sub_ = create_subscription( - // "~/cmd", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - // // If not using PWM control, publish the velocity message directly - // if (!pwm_enabled_) { - // velocity_pub_->publish(*msg); - // } - - // mavros_msgs::msg::OverrideRCIn rc_msg; - - // for (uint16_t & channel : rc_msg.channels) { - // channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; - // } - - // // Scale the velocity commands to the PWM range - // rc_msg.channels[4] = scale_cmd(msg->linear.x, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); - // rc_msg.channels[5] = scale_cmd(-1 * msg->linear.y, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); - // rc_msg.channels[2] = scale_cmd(msg->linear.z, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); - // rc_msg.channels[3] = scale_cmd(-1 * msg->angular.z, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); - - // rc_override_pub_->publish(rc_msg); - // }); + this->declare_parameter("min_pwm", 1100); + this->declare_parameter("max_pwm", 1900); + + pwm_range_ = std::make_tuple(this->get_parameter("min_pwm").as_int(), this->get_parameter("max_pwm").as_int()); + + enable_pwm_service_ = create_service( + "~/enable_pwm_control", [this]( + const std::shared_ptr request, // NOLINT + std::shared_ptr response) { // NOLINT + pwm_enabled_ = request->data; + response->success = true; + return; + }); + + const auto qos_profile = rclcpp::SystemDefaultsQoS(); + velocity_pub_ = create_publisher("/cmd_vel", qos_profile); + rc_override_pub_ = create_publisher("/mavros/rc/override", qos_profile); + + cmd_sub_ = create_subscription( + "~/cmd", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT + // If not using PWM control, publish the velocity message directly + if (!pwm_enabled_) { + velocity_pub_->publish(*msg); + } + + mavros_msgs::msg::OverrideRCIn rc_msg; + + for (uint16_t & channel : rc_msg.channels) { + channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; + } + + // Scale the velocity commands to the PWM range + rc_msg.channels[4] = scale_cmd(msg->linear.x, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); + rc_msg.channels[5] = scale_cmd(-1 * msg->linear.y, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); + rc_msg.channels[2] = scale_cmd(msg->linear.z, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); + rc_msg.channels[3] = scale_cmd(-1 * msg->angular.z, -1.0, 1.0, std::get<0>(pwm_range_), std::get<1>(pwm_range_)); + + rc_override_pub_->publish(rc_msg); + }); } } // namespace ardusub_teleop int main(int argc, char ** argv) { - // rclcpp::init(argc, argv); - // rclcpp::spin(std::make_shared()); - // rclcpp::shutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/thruster_hardware/src/hardware.cpp b/thruster_hardware/src/hardware.cpp index 1fa24cf..0e614b4 100644 --- a/thruster_hardware/src/hardware.cpp +++ b/thruster_hardware/src/hardware.cpp @@ -28,117 +28,117 @@ namespace thruster_hardware hardware_interface::CallbackReturn ThrusterHardware::on_init(const hardware_interface::HardwareInfo & info) { - // RCLCPP_INFO(rclcpp::get_logger("ThrusterHardware"), "Initializing ThrusterHardware system interface."); // NOLINT - - // if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { - // RCLCPP_ERROR(rclcpp::get_logger("ThrusterHardware"), "Failed to initialize the hardware interface."); // NOLINT - // return hardware_interface::CallbackReturn::ERROR; - // } - - // hw_commands_pwm_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - // // Verify that the command and state interface configurations are correct - // for (const auto & joint : info_.joints) { - // if (joint.command_interfaces.size() != 1) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has %ld command interfaces. 1 expected.", - // joint.name.c_str(), joint.command_interfaces.size()); - // return hardware_interface::CallbackReturn::ERROR; - // } - - // if (joint.command_interfaces[0].name != "pwm") { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has command interface '%s'. '%s' expected.", - // joint.name.c_str(), joint.command_interfaces[0].name.c_str(), "pwm"); - // return hardware_interface::CallbackReturn::ERROR; - // } - - // if (!joint.state_interfaces.empty()) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has %ld state interfaces. 0 expected.", joint.name.c_str(), - // joint.state_interfaces.size()); - // return hardware_interface::CallbackReturn::ERROR; - // } - // } - - // // Get the maximum number of attempts that can be made to set the thruster parameters before failing - // if (info_.hardware_parameters.find("max_set_param_attempts") == info_.hardware_parameters.cend()) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "The 'max_set_param_attempts' parameter is required."); - // return hardware_interface::CallbackReturn::ERROR; - // } - // max_retries_ = std::stoi(info_.hardware_parameters.at("max_set_param_attempts")); - - // // Store the thruster configurations - // thruster_configs_.reserve(info_.joints.size()); - - // for (const auto & joint : info_.joints) { - // // Make sure the the joint-level parameters exist - // if ( - // joint.parameters.find("param_name") == joint.parameters.cend() || - // joint.parameters.find("default_param_value") == joint.parameters.cend() || - // joint.parameters.find("channel") == joint.parameters.cend()) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), - // "Joint %s missing required configurations. Ensure that the `param_name`, `default_param_value`, and " - // "`channel` are provided for each joint.", - // joint.name.c_str()); - // return hardware_interface::CallbackReturn::ERROR; - // } - - // ThrusterConfig config; - - // config.param.name = joint.parameters.at("param_name"); - // config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - // config.param.value.integer_value = std::stoi(joint.parameters.at("default_param_value")); - // config.channel = std::stoi(joint.parameters.at("channel")); - - // if (joint.parameters.find("neutral_pwm") != joint.parameters.cend()) { - // config.neutral_pwm = std::stoi(joint.parameters.at("neutral_pwm")); - // } - - // thruster_configs_.emplace_back(config); - // } - - // // Construct a node to use for interacting with MAVROS - // rclcpp::NodeOptions options; - // options.arguments({"--ros-args", "-r", "__node:=thruster_hardware" + info_.name}); - // node_ = rclcpp::Node::make_shared("_", options); - - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Successfully initialized ThrusterHardware system interface!"); + RCLCPP_INFO(rclcpp::get_logger("ThrusterHardware"), "Initializing ThrusterHardware system interface."); // NOLINT + + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("ThrusterHardware"), "Failed to initialize the hardware interface."); // NOLINT + return hardware_interface::CallbackReturn::ERROR; + } + + hw_commands_pwm_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + // Verify that the command and state interface configurations are correct + for (const auto & joint : info_.joints) { + if (joint.command_interfaces.size() != 1) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has %ld command interfaces. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != "pwm") { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has command interface '%s'. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), "pwm"); + return hardware_interface::CallbackReturn::ERROR; + } + + if (!joint.state_interfaces.empty()) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Joint '%s' has %ld state interfaces. 0 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // Get the maximum number of attempts that can be made to set the thruster parameters before failing + if (info_.hardware_parameters.find("max_set_param_attempts") == info_.hardware_parameters.cend()) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "The 'max_set_param_attempts' parameter is required."); + return hardware_interface::CallbackReturn::ERROR; + } + max_retries_ = std::stoi(info_.hardware_parameters.at("max_set_param_attempts")); + + // Store the thruster configurations + thruster_configs_.reserve(info_.joints.size()); + + for (const auto & joint : info_.joints) { + // Make sure the the joint-level parameters exist + if ( + joint.parameters.find("param_name") == joint.parameters.cend() || + joint.parameters.find("default_param_value") == joint.parameters.cend() || + joint.parameters.find("channel") == joint.parameters.cend()) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), + "Joint %s missing required configurations. Ensure that the `param_name`, `default_param_value`, and " + "`channel` are provided for each joint.", + joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + ThrusterConfig config; + + config.param.name = joint.parameters.at("param_name"); + config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + config.param.value.integer_value = std::stoi(joint.parameters.at("default_param_value")); + config.channel = std::stoi(joint.parameters.at("channel")); + + if (joint.parameters.find("neutral_pwm") != joint.parameters.cend()) { + config.neutral_pwm = std::stoi(joint.parameters.at("neutral_pwm")); + } + + thruster_configs_.emplace_back(config); + } + + // Construct a node to use for interacting with MAVROS + rclcpp::NodeOptions options; + options.arguments({"--ros-args", "-r", "__node:=thruster_hardware" + info_.name}); + node_ = rclcpp::Node::make_shared("_", options); + + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Successfully initialized ThrusterHardware system interface!"); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn ThrusterHardware::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Configuring the ThrusterHardware system interface."); - - // override_rc_pub_ = - // node_->create_publisher("mavros/rc/override", rclcpp::SystemDefaultsQoS()); - // rt_override_rc_pub_ = - // std::make_unique>(override_rc_pub_); - - // rt_override_rc_pub_->lock(); - // for (auto & channel : rt_override_rc_pub_->msg_.channels) { - // channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; - // } - // rt_override_rc_pub_->unlock(); - - // set_params_client_ = node_->create_client("mavros/param/set_parameters"); - - // using namespace std::chrono_literals; - // while (!set_params_client_->wait_for_service(1s)) { - // if (!rclcpp::ok()) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Interrupted while waiting for the `mavros/set_parameters` service."); - // return hardware_interface::CallbackReturn::ERROR; - // } - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Waiting for the `mavros/set_parameters` service to be available..."); - // } + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Configuring the ThrusterHardware system interface."); + + override_rc_pub_ = + node_->create_publisher("mavros/rc/override", rclcpp::SystemDefaultsQoS()); + rt_override_rc_pub_ = + std::make_unique>(override_rc_pub_); + + rt_override_rc_pub_->lock(); + for (auto & channel : rt_override_rc_pub_->msg_.channels) { + channel = mavros_msgs::msg::OverrideRCIn::CHAN_NOCHANGE; + } + rt_override_rc_pub_->unlock(); + + set_params_client_ = node_->create_client("mavros/param/set_parameters"); + + using namespace std::chrono_literals; + while (!set_params_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Interrupted while waiting for the `mavros/set_parameters` service."); + return hardware_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Waiting for the `mavros/set_parameters` service to be available..."); + } return hardware_interface::CallbackReturn::SUCCESS; } @@ -150,114 +150,114 @@ hardware_interface::CallbackReturn ThrusterHardware::on_cleanup(const rclcpp_lif void ThrusterHardware::stop_thrusters() { - // if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { - // for (size_t i = 0; i < hw_commands_pwm_.size(); ++i) { - // rt_override_rc_pub_->msg_.channels[thruster_configs_[i].channel - 1] = thruster_configs_[i].neutral_pwm; - // } - // rt_override_rc_pub_->unlockAndPublish(); - // } + if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { + for (size_t i = 0; i < hw_commands_pwm_.size(); ++i) { + rt_override_rc_pub_->msg_.channels[thruster_configs_[i].channel - 1] = thruster_configs_[i].neutral_pwm; + } + rt_override_rc_pub_->unlockAndPublish(); + } } hardware_interface::CallbackReturn ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Activating the ThrusterHardware system interface."); - - // std::vector params; - // params.reserve(thruster_configs_.size()); - - // for (const auto & config : thruster_configs_) { - // rcl_interfaces::msg::Parameter param; - // param.name = config.param.name; - // param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - // param.value.integer_value = 1; // Set the thruster parameter values to RC passthrough here - // params.emplace_back(param); - // } - - // auto request = std::make_shared(); - // request->parameters = params; - - // for (int i = 0; i < max_retries_; ++i) { - // RCLCPP_WARN( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Attempting to set thruster parameters to RC passthrough..."); - - // auto future = set_params_client_->async_send_request(request); - - // // Wait until the result is available - // if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { - // const auto responses = future.get()->results; - // for (const auto & response : responses) { - // if (!response.successful) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Failed to set thruster parameter '%s'.", response.reason.c_str()); - // return hardware_interface::CallbackReturn::ERROR; - // } - // } - - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Successfully set thruster parameters to RC passthrough!"); - - // // Stop the thrusters before switching to an external controller - // stop_thrusters(); - - // return hardware_interface::CallbackReturn::SUCCESS; - // } - // } - - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), - // "Failed to set thruster parameters to passthrough mode after %d attempts. Make sure that the MAVROS parameter " - // "plugin is fully running and configured.", - // max_retries_); + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Activating the ThrusterHardware system interface."); + + std::vector params; + params.reserve(thruster_configs_.size()); + + for (const auto & config : thruster_configs_) { + rcl_interfaces::msg::Parameter param; + param.name = config.param.name; + param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + param.value.integer_value = 1; // Set the thruster parameter values to RC passthrough here + params.emplace_back(param); + } + + auto request = std::make_shared(); + request->parameters = params; + + for (int i = 0; i < max_retries_; ++i) { + RCLCPP_WARN( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Attempting to set thruster parameters to RC passthrough..."); + + auto future = set_params_client_->async_send_request(request); + + // Wait until the result is available + if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { + const auto responses = future.get()->results; + for (const auto & response : responses) { + if (!response.successful) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Failed to set thruster parameter '%s'.", response.reason.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Successfully set thruster parameters to RC passthrough!"); + + // Stop the thrusters before switching to an external controller + stop_thrusters(); + + return hardware_interface::CallbackReturn::SUCCESS; + } + } + + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), + "Failed to set thruster parameters to passthrough mode after %d attempts. Make sure that the MAVROS parameter " + "plugin is fully running and configured.", + max_retries_); return hardware_interface::CallbackReturn::ERROR; } hardware_interface::CallbackReturn ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - // RCLCPP_INFO(rclcpp::get_logger("ThrusterHardware"), "Activating the ThrusterHardware system interface."); // NOLINT + RCLCPP_INFO(rclcpp::get_logger("ThrusterHardware"), "Activating the ThrusterHardware system interface."); // NOLINT - // // Stop the thrusters before switching out of passthrough mode - // stop_thrusters(); + // Stop the thrusters before switching out of passthrough mode + stop_thrusters(); - // std::vector params; - // params.reserve(thruster_configs_.size()); + std::vector params; + params.reserve(thruster_configs_.size()); - // for (const auto & config : thruster_configs_) { - // params.emplace_back(config.param); - // } + for (const auto & config : thruster_configs_) { + params.emplace_back(config.param); + } - // auto request = std::make_shared(); - // request->parameters = params; + auto request = std::make_shared(); + request->parameters = params; - // for (int i = 0; i < max_retries_; ++i) { - // RCLCPP_WARN(rclcpp::get_logger("ThrusterHardware"), "Attempting to leave RC passthrough mode..."); // NOLINT + for (int i = 0; i < max_retries_; ++i) { + RCLCPP_WARN(rclcpp::get_logger("ThrusterHardware"), "Attempting to leave RC passthrough mode..."); // NOLINT - // auto future = set_params_client_->async_send_request(request); + auto future = set_params_client_->async_send_request(request); - // // Wait until the result is available - // if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { - // const auto responses = future.get()->results; - // for (const auto & response : responses) { - // if (!response.successful) { - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Failed to set thruster parameter '%s'.", response.reason.c_str()); - // return hardware_interface::CallbackReturn::ERROR; - // } - // } + // Wait until the result is available + if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { + const auto responses = future.get()->results; + for (const auto & response : responses) { + if (!response.successful) { + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Failed to set thruster parameter '%s'.", response.reason.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } - // RCLCPP_INFO( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), "Successfully restored the default thruster values!"); + RCLCPP_INFO( // NOLINT + rclcpp::get_logger("ThrusterHardware"), "Successfully restored the default thruster values!"); - // return hardware_interface::CallbackReturn::SUCCESS; - // } - // } + return hardware_interface::CallbackReturn::SUCCESS; + } + } - // RCLCPP_ERROR( // NOLINT - // rclcpp::get_logger("ThrusterHardware"), - // "Failed to fully leave passthrough mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " - // "running and configured.", - // max_retries_); + RCLCPP_ERROR( // NOLINT + rclcpp::get_logger("ThrusterHardware"), + "Failed to fully leave passthrough mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " + "running and configured.", + max_retries_); return hardware_interface::CallbackReturn::ERROR; } @@ -289,12 +289,12 @@ hardware_interface::return_type ThrusterHardware::read( hardware_interface::return_type ThrusterHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { - // for (size_t i = 0; i < hw_commands_pwm_.size(); ++i) { - // rt_override_rc_pub_->msg_.channels[thruster_configs_[i].channel - 1] = static_cast(hw_commands_pwm_[i]); - // } - // rt_override_rc_pub_->unlockAndPublish(); - // } + if (rt_override_rc_pub_ && rt_override_rc_pub_->trylock()) { + for (size_t i = 0; i < hw_commands_pwm_.size(); ++i) { + rt_override_rc_pub_->msg_.channels[thruster_configs_[i].channel - 1] = static_cast(hw_commands_pwm_[i]); + } + rt_override_rc_pub_->unlockAndPublish(); + } return hardware_interface::return_type::OK; }