From bec46a991fae2db1f3f63c8eb34d34e843c5b0ec Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 24 Jun 2024 15:40:35 -0700 Subject: [PATCH] Changes to ensure build works --- ardusub_manager/src/ardusub_manager.cpp | 252 +++++++-------- ardusub_teleop/src/joy_interface.cpp | 84 ++--- thruster_hardware/src/hardware.cpp | 408 ++++++++++++------------ 3 files changed, 372 insertions(+), 372 deletions(-) diff --git a/ardusub_manager/src/ardusub_manager.cpp b/ardusub_manager/src/ardusub_manager.cpp index 11a973a..fd1deb1 100644 --- a/ardusub_manager/src/ardusub_manager.cpp +++ b/ardusub_manager/src/ardusub_manager.cpp @@ -68,156 +68,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", 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 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; + // using namespace std::chrono_literals; - // 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 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 +236,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 6d88c87..3a533a7 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 0e614b4..906d32f 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; } @@ -270,12 +270,12 @@ std::vector ThrusterHardware::export_state_i std::vector ThrusterHardware::export_command_interfaces() { - std::vector command_interfaces; - command_interfaces.reserve(info_.joints.size()); + // std::vector command_interfaces; + // command_interfaces.reserve(info_.joints.size()); - for (size_t i = 0; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(info_.joints[i].name, "pwm", &hw_commands_pwm_[i]); - } + // for (size_t i = 0; i < info_.joints.size(); ++i) { + // command_interfaces.emplace_back(info_.joints[i].name, "pwm", &hw_commands_pwm_[i]); + // } return command_interfaces; } @@ -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; }