rotors driver first prototype

drone_wrapper/src/drone_wrapper/drone_wrapper_class.py

@@ -100,7 +100,6 @@ def __transform_twist(self, from_, to, twist):
             r --> rotations
             t --> translations
             s --> scale
-
         vin = np.array([twist.linear.x, twist.linear.y, twist.linear.z])
         result = np.dot(vin, H[:3, :3])
         """
@@ -673,4 +672,4 @@ def __init__(self, name='drone', ns='', verbose=False):
 
 if __name__ == "__main__":
     drone = DroneWrapper()
-    rospy.spin()
+    rospy.spin()

jderobot_drones/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>drone_assets</exec_depend>
   <exec_depend>drone_circuit_assets</exec_depend>
   <exec_depend>tello_driver</exec_depend>
+  <exec_depend>rotors_driver</exec_depend>
 
   <export>
     <metapackage/>

rotors_driver/CHANGELOG.rst

@@ -0,0 +1,59 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rotors_driver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.4.2 (2021-10-04)
+------------------
+* Added Rotors driver for rotors sim
+* Contributors: RUFFY-369
+
+1.4.1 (2021-09-17)
+------------------
+
+1.4.0 (2021-04-23)
+------------------
+
+1.3.8 (2021-04-14)
+------------------
+
+1.3.7 (2021-03-01)
+------------------
+
+1.3.6 (2021-01-12)
+------------------
+
+1.3.5 (2020-12-29)
+------------------
+
+1.3.4 (2020-06-28)
+------------------
+
+1.3.3 (2020-06-10)
+------------------
+
+1.3.2 (2020-05-06)
+------------------
+
+1.3.1 (2020-02-14 13:30)
+------------------------
+
+1.3.0 (2020-02-14 13:22)
+------------------------
+
+1.2.0 (2020-02-11)
+------------------
+
+1.1.0 (2020-01-20)
+------------------
+
+1.0.2 (2020-01-19)
+------------------
+
+1.0.1 (2019-08-18)
+------------------
+
+1.0.0 (2019-08-01)
+------------------
+
+0.0.1 (2019-07-20)
+------------------

rotors_driver/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rotors_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  geometry_msgs
+  mavros_msgs
+  mavros
+  nav_msgs
+  sensor_msgs
+  std_msgs
+  trajectory_msgs
+  tf
+)
+
+catkin_package()
+
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+install(PROGRAMS
+  src/rotors_driver_node.py
+  scripts/rotors_land
+  scripts/rotors_takeoff
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

rotors_driver/launch/rotors_driver.launch

@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+	<node name="rotors_driver_node" pkg="rotors_driver" type="rotors_driver_node.py" output="screen"/>
+</launch>

rotors_driver/launch/rotors_wrapper.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+    <!--    Drone model param   -->
+    <arg name="drone_model" default="firefly"/>
+    <param name="drone_model" type="str" value="$(arg drone_model)" />
+
+	<node name="rotors_driver_node" pkg="rotors_driver" type="rotors_driver_node.py" output="screen"/>
+    <node pkg="drone_wrapper" type="drone_wrapper_class.py" name="drone_wrapper_node" output="screen"/>
+</launch>

rotors_driver/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rotors_driver</name>
+  <version>1.4.2</version>
+  <description>The rotors_driver package</description>
+
+  <maintainer email="[email protected]">Pedro Arias</maintainer>
+  <author email="[email protected]">Prakarsh Kaushik</author>
+
+  <license>MIT</license>
+
+  <!-- <url type="website">WIP</url> -->
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rospy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>mavros_msgs</depend>
+  <depend>mavros</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>trajectory_msgs</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rotors_driver/scripts/rotors_land

@@ -0,0 +1,2 @@
+#! /bin/sh
+rosservice call /mavros/cmd/land 0.0 0.0 0.0 0.0 0.0

rotors_driver/scripts/rotors_takeoff

@@ -0,0 +1,2 @@
+#! /bin/sh
+rosservice call /mavros/cmd/arming 0.0 0.0 0.0 0.0 0.0