Created estop

CMakeLists.txt

@@ -62,6 +62,18 @@ install(
   INCLUDES DESTINATION include/${PROJECT_NAME}
 )
 
+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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+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)

src/estop.cpp

@@ -0,0 +1,87 @@
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+
+#include "px4_msgs/msg/vehicle_command.hpp"
+
+using namespace std::chrono_literals;
+
+class Estop : public rclcpp::Node {
+
+    public:
+    Estop() 
+        : Node("estop")
+    {
+        for (int i=0; i < maxN; i++){
+            std::string topic_name = "/drone" + std::to_string(i) +  "/fmu/vehicle_command/in";
+            drone_publishers_.push_back(
+                    this->create_publisher<px4_msgs::msg::VehicleCommand>(topic_name,10)
+                    );
+        }
+
+        for (int i=0; i < maxN; i++){
+            std::string topic_name = "/rover" + std::to_string(i) + "/fmu/vehicle_command/in";
+            rover_publishers_.push_back(
+                    this->create_publisher<px4_msgs::msg::VehicleCommand>(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<rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr> drone_publishers_;
+    std::vector<rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::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<Estop>());
+
+    rclcpp::shutdown();
+    return 0;
+}