Skip to content

Commit

Permalink
Changes to ensure build works
Browse files Browse the repository at this point in the history
  • Loading branch information
@evan-palmer
evan-palmer committed Jun 24, 2024
1 parent 8deb3a7 commit bec46a9
Show file tree
Hide file tree
Showing 3 changed files with 372 additions and 372 deletions.
252 changes: 126 additions & 126 deletions ardusub_manager/src/ardusub_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,156 +68,156 @@ ArduSubManager::ArduSubManager() // NOLINT

void ArduSubManager::set_message_rate(int64_t msg_id, double rate) const
{
auto request = std::make_shared<mavros_msgs::srv::MessageInterval::Request>();
request->message_id = msg_id;
request->message_rate = rate;
// auto request = std::make_shared<mavros_msgs::srv::MessageInterval::Request>();
// 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<int64_t> & msg_ids, const std::vector<double> & 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<ardusub_manager::ParamListener>(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_msgs::srv::MessageInterval>(
"/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<geographic_msgs::msg::GeoPointStamped>("/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<tf2_ros::TransformBroadcaster>(*this);

pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"/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<ardusub_manager::ParamListener>(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_msgs::srv::MessageInterval>(
// "/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<geographic_msgs::msg::GeoPointStamped>("/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<tf2_ros::TransformBroadcaster>(*this);

// pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
// "/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;
}
Expand All @@ -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<ardusub_manager::ArduSubManager>();
executor.add_node(node->get_node_base_interface());
// auto node = std::make_shared<ardusub_manager::ArduSubManager>();
// executor.add_node(node->get_node_base_interface());

executor.spin();
// executor.spin();

rclcpp::shutdown();
// rclcpp::shutdown();

return 0;
}
84 changes: 42 additions & 42 deletions ardusub_teleop/src/joy_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std_srvs::srv::SetBool>(
"~/enable_pwm_control", [this](
const std::shared_ptr<std_srvs::srv::SetBool::Request> request, // NOLINT
std::shared_ptr<std_srvs::srv::SetBool::Response> response) { // NOLINT
pwm_enabled_ = request->data;
response->success = true;
return;
});

const auto qos_profile = rclcpp::SystemDefaultsQoS();
velocity_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", qos_profile);
rc_override_pub_ = create_publisher<mavros_msgs::msg::OverrideRCIn>("/mavros/rc/override", qos_profile);

cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
"~/cmd", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Twist> 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<std_srvs::srv::SetBool>(
// "~/enable_pwm_control", [this](
// const std::shared_ptr<std_srvs::srv::SetBool::Request> request, // NOLINT
// std::shared_ptr<std_srvs::srv::SetBool::Response> response) { // NOLINT
// pwm_enabled_ = request->data;
// response->success = true;
// return;
// });

// const auto qos_profile = rclcpp::SystemDefaultsQoS();
// velocity_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", qos_profile);
// rc_override_pub_ = create_publisher<mavros_msgs::msg::OverrideRCIn>("/mavros/rc/override", qos_profile);

// cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
// "~/cmd", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Twist> 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<ardusub_teleop::JoyInterface>());
rclcpp::shutdown();
// rclcpp::init(argc, argv);
// rclcpp::spin(std::make_shared<ardusub_teleop::JoyInterface>());
// rclcpp::shutdown();
return 0;
}
Loading

0 comments on commit bec46a9

Please sign in to comment.