From 97fcc6bd2ffd2fd2e4d2e4f7054283de7318c6f9 Mon Sep 17 00:00:00 2001 From: Devansh Ramgopal Agrawal Date: Wed, 14 Sep 2022 20:45:21 -0400 Subject: [PATCH 1/3] created estop - untested --- src/estop.cpp | 87 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 src/estop.cpp diff --git a/src/estop.cpp b/src/estop.cpp new file mode 100644 index 0000000..e8dda5e --- /dev/null +++ b/src/estop.cpp @@ -0,0 +1,87 @@ + +#include +#include +#include +#include + + +#include "px4_msgs/msg/vehicle_command.hpp" + + +class Estop : public rclcpp::Node { + + public: + Estop() + : Node("estop") + { + for (int i=0; i < maxN; i++){ + std::string topic_name = std::format("/drone{}/fmu/vehicle_command/in"); + drone_publishers_.push_back( + this->create_publisher(topic_name,10) + ); + } + + for (int i=0; i < maxN; i++){ + std::string topic_name = std::format("/rover{}/fmu/vehicle_command/in"); + rover_publishers_.push_back( + this->create_publisher(topic_name,10) + ); + } + + timer_ = this->create_wall_timer( + 200ms, std::bind(&Estop::timer_callback, this)); + } + + private: + + const int maxN = 25; + + rclcpp::TimerBase::SharedPtr timer_; + std::vector::SharedPtr> drone_publishers_; + std::vector::SharedPtr> rover_publishers_; + + + void timer_callback() { + + auto msg = px4_msgs::msg::VehicleCommand(); + msg.timestamp = 0; + msg.param1 = 0.0; + msg.param2 = 21196; // force disarm + msg.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM; + msg.target_component = 1; + msg.source_system = 1; + msg.source_component = 1; + msg.from_external = true; + + // drone stop command + for (int i=0; i< maxN; i++){ + msg.target_system = i; + drone_publishers[i]->publish(msg); + } + + // rover stop command + for (int i=0; i< maxN; i++){ + msg.target_system = i; + rover_publishers[i]->publish(msg); + } + + RCLCPP_WARN(this->get_logger(), "sending e stop to drones with id less than %d", maxN); + } + + + +}; // class Estop + + +int main(int argc, char* argv[]) { + + std::cout << "Sending ESTOP" << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + return 0; +} From fd52f5ec4fbb7bedbd9aff4a3bfd4d548d9a094d Mon Sep 17 00:00:00 2001 From: Devansh Ramgopal Agrawal Date: Wed, 14 Sep 2022 20:46:37 -0400 Subject: [PATCH 2/3] added CMakeLists --- CMakeLists.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a97dab..bcc4890 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,18 @@ install( INCLUDES DESTINATION include/${PROJECT_NAME} ) +add_executable(estop src/example_control.cpp) +ament_target_dependencies(estop rclcpp px4_msgs geometry_msgs tf2 tf2_ros px4_ros_com) +target_link_libraries(estop dascBots) +target_include_directories(estop PUBLIC + $ + $ +) +install(TARGETS + estop + DESTINATION lib/${PROJECT_NAME} +) + add_executable(example_control src/example_control.cpp) ament_target_dependencies(example_control rclcpp px4_msgs geometry_msgs tf2 tf2_ros px4_ros_com) target_link_libraries(example_control dascBots) From 3a56dfb808c81d300b6969025af9ddba96f16df3 Mon Sep 17 00:00:00 2001 From: devansh Date: Wed, 14 Sep 2022 20:59:06 -0400 Subject: [PATCH 3/3] working estop --- CMakeLists.txt | 2 +- src/estop.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bcc4890..cabeb7a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,7 +62,7 @@ install( INCLUDES DESTINATION include/${PROJECT_NAME} ) -add_executable(estop src/example_control.cpp) +add_executable(estop src/estop.cpp) ament_target_dependencies(estop rclcpp px4_msgs geometry_msgs tf2 tf2_ros px4_ros_com) target_link_libraries(estop dascBots) target_include_directories(estop PUBLIC diff --git a/src/estop.cpp b/src/estop.cpp index e8dda5e..239ef3b 100644 --- a/src/estop.cpp +++ b/src/estop.cpp @@ -2,11 +2,11 @@ #include #include #include -#include - +#include "rclcpp/rclcpp.hpp" #include "px4_msgs/msg/vehicle_command.hpp" +using namespace std::chrono_literals; class Estop : public rclcpp::Node { @@ -15,14 +15,14 @@ class Estop : public rclcpp::Node { : Node("estop") { for (int i=0; i < maxN; i++){ - std::string topic_name = std::format("/drone{}/fmu/vehicle_command/in"); + std::string topic_name = "/drone" + std::to_string(i) + "/fmu/vehicle_command/in"; drone_publishers_.push_back( this->create_publisher(topic_name,10) ); } for (int i=0; i < maxN; i++){ - std::string topic_name = std::format("/rover{}/fmu/vehicle_command/in"); + std::string topic_name = "/rover" + std::to_string(i) + "/fmu/vehicle_command/in"; rover_publishers_.push_back( this->create_publisher(topic_name,10) ); @@ -56,13 +56,13 @@ class Estop : public rclcpp::Node { // drone stop command for (int i=0; i< maxN; i++){ msg.target_system = i; - drone_publishers[i]->publish(msg); + drone_publishers_[i]->publish(msg); } // rover stop command for (int i=0; i< maxN; i++){ msg.target_system = i; - rover_publishers[i]->publish(msg); + rover_publishers_[i]->publish(msg); } RCLCPP_WARN(this->get_logger(), "sending e stop to drones with id less than %d", maxN);