remove checking of nan

call_attitude.sh

@@ -1 +0,0 @@
-ros2 service call /drone/set_attitude drone_services/srv/SetAttitude "{pitch: $1, yaw: $2, roll: $3, thrust: $4}"

src/drone_services/CMakeLists.txt

@@ -28,7 +28,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetVehicleControl.srv"
   "srv/ArmDrone.srv"
   "srv/DisarmDrone.srv"
-  "srv/ControlRelais.srv"
   "msg/DroneControlMode.msg"
 )
 

src/drone_services/msg/DroneArmStatus.msg

@@ -1 +0,0 @@
-bool armed false

src/drone_services/msg/DroneRouteStatus.msg

@@ -1,2 +0,0 @@
-int32 current_setpoint_index
-float32[5] current_setpoint # x,y,z,angle,take_picture

src/drone_services/msg/DroneStatus.msg

@@ -1,5 +0,0 @@
-float32 battery_percentage
-float32 cpu_usage
-int32 route_setpoint # -1 if no route
-wstring control_mode
-bool armed

src/drone_services/srv/ControlRelais.srv

@@ -1,5 +0,0 @@
-#to turn the relais on or off
-bool relais1_on false
-bool relais2_on false
----
-int8 bits # relais 1 = bit 0, relais 2 is bit 1

src/drone_services/srv/SetTrajectory.srv

@@ -1,7 +1,10 @@
 # control mode velocity or position, refer to drone_services/msg/DroneControlMode.msg
 int8 control_mode
 # x, y, z values
-float32[3] values
+#float32[3] values
+float32 x
+float32 y
+float32 z
 float32 yaw
 ---
 bool success

src/drone_status/drone_status/drone_status.py

@@ -1,63 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-from drone_services.msg import DroneStatus
-from drone_services.msg import DroneControlMode
-from drone_services.msg import DroneArmStatus
-from drone_services.msg import DroneRouteStatus
-from px4_msgs.msg import BatteryStatus
-from px4_msgs.msg import Cpuload
-
-CONTROL_MODE_ATTITUDE = 1
-CONTROL_MODE_VELOCITY = 2
-CONTROL_MODE_POSITION = 3
-
-class DroneStatus(Node):
-    def __init__(self):
-        super().__init__('drone_status')
-        #publish to drone/status topic
-        self.publisher = self.create_publisher(DroneStatus, '/drone/status', 10)
-        self.control_mode_subscriber = self.create_subscription(DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10)
-        self.arm_status_subscriber = self.create_subscription(DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10)
-        self.route_status_subscriber = self.create_subscription(DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10)
-        self.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10)
-        self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10)
-        #publish every 0.5 seconds
-        self.timer = self.create_timer(0.5, self.publish_status)
-        self.armed = False
-        self.control_mode = "attitude"
-        self.battery_percentage = 100.0
-        self.cpu_usage = 0.0
-        self.route_setpoint = 0
-    
-    def publish_status(self):
-        msg = DroneStatus()
-        msg.armed = self.armed
-        msg.control_mode = self.control_mode
-        msg.battery_percentage = self.battery_percentage
-        msg.cpu_usage = self.cpu_usage
-        msg.route_setpoint = self.route_setpoint
-        self.publisher.publish(msg)
-        self.get_logger().info('Publishing: "%s"' % msg.status)
-
-    def control_mode_callback(self,msg):
-        if msg.control_mode == CONTROL_MODE_ATTITUDE:
-            self.control_mode = "attitude"
-        elif msg.control_mode == CONTROL_MODE_VELOCITY:
-            self.control_mode = "velocity"
-        elif msg.control_mode == CONTROL_MODE_POSITION:
-            self.control_mode = "position"
-        else:
-            self.control_mode = "unknown"
-
-    def arm_status_callback(self,msg):
-        self.armed = msg.armed
-    
-    def route_status_callback(self,msg):
-        self.route_setpoint = msg.current_setpoint_index
-
-    def battery_status_callback(self, msg):
-        self.battery_percentage = msg.remaining * 100.0
-    
-    def cpu_load_callback(self, msg):
-        self.cpu_usage = msg.load

src/drone_status/package.xml

@@ -1,21 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-    <name>drone_status</name>
-    <version>0.0.0</version>
-    <description>Package for combining several data points from the drone into a single topic</description>
-    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-    <license>Apache License 2.0</license>
-    <exec_depend>rclpy</exec_depend>
-    <exec_depend>drone_services</exec_depend>
-    <exec_depend>px4_msgs</exec_depend>
-
-    <test_depend>ament_copyright</test_depend>
-    <test_depend>ament_flake8</test_depend>
-    <test_depend>ament_pep257</test_depend>
-    <test_depend>python3-pytest</test_depend>
-
-    <export>
-        <build_type>ament_python</build_type>
-    </export>
-</package>

src/drone_status/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script-dir=$base/lib/drone_status
-[install]
-install-scripts=$base/lib/drone_status

src/drone_status/setup.py

@@ -1,25 +0,0 @@
-from setuptools import setup
-
-package_name = 'drone_status'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=[package_name],
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='ubuntu',
-    maintainer_email='semmer99@gmail.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-        ],
-    },
-)

src/drone_status/test/test_copyright.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'

src/drone_status/test/test_flake8.py

@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)

src/drone_status/test/test_pep257.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'

src/px4_connection/CMakeLists.txt

@@ -28,7 +28,6 @@ add_executable(heartbeat src/heartbeat.cpp)
 ament_target_dependencies(
     heartbeat
     rclcpp
-    drone_services
     px4_ros_com
     px4_msgs
     drone_services

src/px4_connection/src/heartbeat.cpp

@@ -1,4 +1,3 @@
-
 /**
  * @file heartbeat.cpp
  * @author Sem van der Hoeven (sem.hoeven@gmail.com)
@@ -63,6 +62,15 @@ private:
         msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false;
         msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false;
         msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false;
+        
+        /*
+        msg.position = false;
+        msg.velocity = false;
+        msg.acceleration = false;
+        msg.attitude = true;
+        msg.body_rate = false;
+        msg.actuator = false;
+        */
         // get timestamp and publish message
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
         offboard_control_mode_publisher_->publish(msg);

src/px4_connection/src/px4_controller.cpp

@@ -15,14 +15,12 @@
 #include <px4_msgs/msg/vehicle_command.hpp>
 #include <px4_msgs/msg/vehicle_control_mode.hpp>
 #include <px4_msgs/msg/vehicle_attitude.hpp>
-#include <px4_msgs/msg/vehicle_local_position.hpp>
 
 #include <drone_services/srv/set_attitude.hpp>
 #include <drone_services/srv/set_trajectory.hpp>
 #include <drone_services/srv/arm_drone.hpp>
 #include <drone_services/srv/disarm_drone.hpp>
 #include <drone_services/msg/drone_control_mode.hpp>
-#include <drone_services/msg/drone_arm_status.hpp>
 
 #include <std_srvs/srv/empty.hpp>
 
@@ -46,14 +44,11 @@ public:
         vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
         vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
         trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
-        arm_status_publisher_ = this->create_publisher<drone_services::msg::DroneArmStatus>("/drone/arm_status", 10);
         // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
 
         vehicle_attitude_subscription_ = this->create_subscription<px4_msgs::msg::VehicleAttitude>("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1));
-        vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
         control_mode_subscription_ = this->create_subscription<drone_services::msg::DroneControlMode>("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1));
 
-
         set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         set_trajectory_service_ = this->create_service<drone_services::srv::SetTrajectory>("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         disarm_service_ = this->create_service<drone_services::srv::DisarmDrone>("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
@@ -69,10 +64,7 @@ private:
     rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
     rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
     rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
-    rclcpp::Publisher<drone_services::msg::DroneArmStatus>::SharedPtr arm_status_publisher_;
-
     rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr vehicle_attitude_subscription_;
-    rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr vehicle_local_position_subscription_;
     rclcpp::Subscription<drone_services::msg::DroneControlMode>::SharedPtr control_mode_subscription_;
 
     rclcpp::Service<drone_services::srv::SetAttitude>::SharedPtr set_attitude_service_;
@@ -104,29 +96,13 @@ private:
     float base_q[4] = {0, 0, 0, 0};
     int base_q_amount = 0;
 
+    bool trajectory_valid_x = false;
+    bool trajectory_valid_y = false;
+    bool trajectory_valid_z = false;
+    bool trajectory_valid_yaw = false;
+
     char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control
 
-    bool start_trajectory = false;
-    const float omega = 0.3; // angular speed of the POLAR trajectory
-    const float K = 0;       // [m] gain that regulates the spiral pitch
-
-    float rho_0 = 0;
-    float theta_0 = 0;
-    const float p0_z = -0.0;
-    float p0_x = rho_0 * cos(theta_0);
-    float p0_y = rho_0 * sin(theta_0);
-    float des_x = p0_x, des_y = p0_y, des_z = p0_z;
-    float dot_des_x = 0.0, dot_des_y = 0.0;
-    float gamma = M_PI_4;
-
-    float X;
-    float Y;
-
-    float local_x = 0;
-    float local_y = 0;
-
-    uint32_t discrete_time_index = 0;
-
     // result quaternion
     std::array<float, 4> q = {0, 0, 0, 0};
 
@@ -185,24 +161,37 @@ private:
         else
         {
             RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode);
+            // trajectory_valid_x = request->x != NAN;
+            // trajectory_valid_y = request->y != NAN;
+            // trajectory_valid_z = request->z != NAN;
+            // trajectory_valid_yaw = request->yaw != NAN;
+
             if (request->control_mode == CONTROL_MODE_VELOCITY)
             {
-                for (int i = 0; i < 3; i++)
-                {
-                    velocity[i] += request->values[i];
-                }
-                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
+                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", request->x, request->y, request->z);
+                // if (trajectory_valid_x)
+                velocity[0] += request->x;
+                // if (trajectory_valid_y)
+                velocity[1] += request->y;
+                // if (trajectory_valid_z)
+                velocity[2] += request->z;
+
+                RCLCPP_INFO(this->get_logger(), "Set new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
             }
             else if (request->control_mode == CONTROL_MODE_POSITION)
             {
-                position[0] += request->values[0];
-                position[1] += request->values[1];
-                position[2] -= request->values[2]; // height is negative
-
-                RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
+                RCLCPP_INFO(this->get_logger(), "Got new position setpoint. %f %f %f", request->x, request->y, request->z);
+                // if (trajectory_valid_x)
+                position[0] = request->x;
+                // if (trajectory_valid_y)
+                position[1] = request->y;
+                // if (trajectory_valid_z)
+                position[2] = request->z;
+                RCLCPP_INFO(this->get_logger(), "Set new position setpoint: %f %f %f", position[0], position[1], position[2]);
             }
 
-            last_angle += request->yaw;
+            if (trajectory_valid_yaw)
+                last_angle = request->yaw;
             new_setpoint = true;
             RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
             response->success = true;
@@ -228,11 +217,6 @@ private:
             armed = false;
             user_in_control = false;
             publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0);
-
-            auto msg = drone_services::msg::DroneArmStatus();
-            msg.armed = false;
-            arm_status_publisher_->publish(msg);
-            
             response->success = true;
         }
         else
@@ -266,11 +250,6 @@ private:
             RCLCPP_INFO(this->get_logger(), "Arm command sent");
             this->last_thrust = -0.1; // spin motors at 10% thrust
             armed = true;
-
-            auto msg = drone_services::msg::DroneArmStatus();
-            msg.armed = true;
-            arm_status_publisher_->publish(msg);
-
             response->success = true;
         }
         else
@@ -310,13 +289,21 @@ private:
     {
         auto msg = px4_msgs::msg::TrajectorySetpoint();
 
+        // if (trajectory_valid_x)
+        // {
         msg.velocity[0] = velocity[0];
+        trajectory_valid_x = false;
+        // }
+        // if (trajectory_valid_y)
+        // {
         msg.velocity[1] = velocity[1];
-        msg.velocity[2] = -velocity[2];
-        for (int i = 0; i < 3; i++)
-        {
-            msg.position[i] = NAN;
-        }
+        trajectory_valid_y = false;
+        // }
+        // if (trajectory_valid_z)
+        // {
+        msg.velocity[2] = D_SPEED(velocity[2]);
+        trajectory_valid_z = false;
+        // }
 
         publish_trajectory_setpoint(msg);
     }
@@ -325,19 +312,32 @@ private:
     {
         auto msg = px4_msgs::msg::TrajectorySetpoint();
 
-        RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
-        RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y);
-        msg.position = {local_x, local_y, des_z};
-        msg.velocity = {dot_des_x, dot_des_y, 0.0};
-        msg.yaw = gamma; //-3.14; // [-PI:PI]
+        // if (trajectory_valid_x)
+        // {
+        msg.position[0] = position[0];
+        trajectory_valid_x = false;
+        // }
+        // if (trajectory_valid_y)
+        // {
+        msg.position[1] = position[1];
+        trajectory_valid_y = false;
+        // }
+        // if (trajectory_valid_z)
+        // {
+        msg.position[2] = position[2];
+        trajectory_valid_z = false;
+        // }
 
-        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
-        trajectory_setpoint_publisher->publish(msg);
+        publish_trajectory_setpoint(msg);
     }
 
     void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
     {
+        // if (trajectory_valid_yaw)
+        // {
         msg.yaw = last_angle;
+        trajectory_valid_yaw = false;
+        // }
         msg.yawspeed = DEFAULT_YAW_SPEED;
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
         trajectory_setpoint_publisher->publish(msg);
@@ -349,24 +349,25 @@ private:
      */
     void send_setpoint()
     {
-        // the spiral, in polar coordinates (theta, rho), is given by
-        // theta = theta_0 + omega*t
-        // rho = rho_0 + K*theta
-        float theta = theta_0 + omega * 0.1 * discrete_time_index;
-        float rho = rho_0 + K * theta;
 
-        // from polar to cartesian coordinates
-        des_x = rho * cos(theta);
-        des_y = rho * sin(theta);
-        des_z = position[2]; // the z position can be set to the received height
+        /*
+        setpoint_count++;
+        if (setpoint_count == 20)
+        {
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
+            RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
+            // arm the drone
+            this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
 
-        // velocity computation
-        float dot_rho = K * omega;
-        dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega;
-        dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega;
-        // desired heading direction
-        gamma = atan2(dot_des_y, dot_des_x);
+            RCLCPP_INFO(this->get_logger(), "Arm command sent");
+            this->last_thrust = -0.1; // spin motors at 10% thrust
+            armed = true;
+        }
 
+        send_attitude_setpoint();
+        */
+
+        // RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
         if (!user_in_control)
         {
             // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
@@ -387,21 +388,17 @@ private:
                 }
                 if (current_control_mode == CONTROL_MODE_VELOCITY)
                 {
-                    // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint");
+                    RCLCPP_INFO(this->get_logger(), "velocity %f %f %f", velocity[0], velocity[1], velocity[2]);
                     send_velocity_setpoint();
                 }
                 else if (current_control_mode == CONTROL_MODE_POSITION)
                 {
-                    // RCLCPP_INFO(this->get_logger(), "Sending position setpoint");
+                    RCLCPP_INFO(this->get_logger(), "position %f %f %f", position[0], position[1], position[2]);
                     send_position_setpoint();
                 }
                 new_setpoint = false;
             }
         }
-        if (start_trajectory)
-        {
-            discrete_time_index++;
-        }
     }
 
     void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
@@ -429,30 +426,7 @@ private:
                 base_q_amount++;
             }
 
-            // RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]);
-        }
-    }
-
-    void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)
-    {
-        // set start values to current position
-        if (!user_in_control)
-        {
-            // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf
-            rho_0 = pow(msg->x,2) + pow(msg->y,2);
-            theta_0 = atan2(msg->y, msg->x);
-            last_angle = msg->heading;
-
-            local_x = msg->x;
-            local_y = msg->y;
-        }
-        X = msg->x;
-        Y = msg->y;
-        float Z = msg->z;
-        if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z))
-        {
-            start_trajectory = true;
-            RCLCPP_INFO(this->get_logger(), "start trajectory");
+            RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]);
         }
     }
 

src/relais_control/package.xml

@@ -1,21 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-    <name>relais_control</name>
-    <version>0.0.0</version>
-    <description>package to control the relais that enables Pixhawk RX and TX communication</description>
-    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-    <license>Apache License 2.0</license>
-    <exec_depend>rclpy</exec_depend>
-    <exec_depend>drone_services</exec_depend>
-
-
-    <test_depend>ament_copyright</test_depend>
-    <test_depend>ament_flake8</test_depend>
-    <test_depend>ament_pep257</test_depend>
-    <test_depend>python3-pytest</test_depend>
-
-    <export>
-        <build_type>ament_python</build_type>
-    </export>
-</package>

src/relais_control/relais_control/relais_controller.py

@@ -1,66 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-try:
-    import RPi.GPIO as GPIO
-except RuntimeError:
-    print("Error importing RPi.GPIO!  This is probably because you need superuser privileges.  You can achieve this by using 'sudo' to run your script")
-
-from drone_services.srv import ControlRelais
-class RelaisController(Node):
-    def __init__(self):
-        super().__init__('relais_controller')
-        self.srv = self.create_service(ControlRelais, '/drone/control_relais', self.control_relais_callback)
-
-        self.relais1_pin = 17
-        self.relais2_pin = 27
-        self.init_gpio()
-        self.turn_relais_on()
-    
-    def init_gpio(self):
-        GPIO.setwarnings(False)
-
-        self.get_logger().info(str(GPIO.RPI_INFO))
-
-        GPIO.setmode(GPIO.BCM)
-
-        GPIO.setup(self.relais1_pin, GPIO.OUT)
-        GPIO.setup(self.relais2_pin, GPIO.OUT)
-        self.get_logger().info("GPIO initialized")
-
-    def turn_relais_on(self):
-        GPIO.output(self.relais1_pin, GPIO.HIGH)
-        GPIO.output(self.relais2_pin, GPIO.HIGH)
-        self.get_logger().info("Relais turned on")
-
-    def control_relais_callback(self, request, response):
-        if request.relais1_on:
-            GPIO.output(self.relais1_pin, GPIO.HIGH)
-            response.bits = response.bits | 1
-        else:
-            GPIO.output(self.relais1_pin, GPIO.LOW)
-            response.bits = response.bits & ~(1 << 0)
-        if request.relais2_on:
-            GPIO.output(self.relais2_pin, GPIO.HIGH)
-            response.bits = response.bits | (1 << 1)
-        else:
-            GPIO.output(self.relais2_pin, GPIO.LOW)
-            response.bits = response.bits & ~(1 << 1)
-        return response
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    relais_controller = RelaisController()
-
-    rclpy.spin(relais_controller)
-
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
-    relais_controller.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

src/relais_control/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script-dir=$base/lib/relais_control
-[install]
-install-scripts=$base/lib/relais_control

src/relais_control/setup.py

@@ -1,26 +0,0 @@
-from setuptools import setup
-
-package_name = 'relais_control'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=[package_name],
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='ubuntu',
-    maintainer_email='semmer99@gmail.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'relais_controller = relais_control.relais_controller:main'
-        ],
-    },
-)

src/relais_control/test/test_copyright.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'

src/relais_control/test/test_flake8.py

@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)

src/relais_control/test/test_pep257.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'

src/test_controls/test_controls/test_controller.py

@@ -3,6 +3,7 @@ from rclpy.node import Node
 
 from sshkeyboard import listen_keyboard_manual
 import asyncio
+from numpy import NAN
 
 from drone_services.srv import SetAttitude
 from drone_services.srv import SetTrajectory
@@ -76,8 +77,20 @@ class TestController(Node):
 
     def send_velocity_request(self, x, y, z, angle):
         self.traj_req.control_mode = 2
-        self.traj_req.yaw = angle
-        self.traj_req.values = [x, y, z]
+        if (angle != NAN):
+            self.traj_req.yaw = angle
+        if (x != NAN):
+            self.traj_req.x = x
+        else:
+            self.traj_req.x = 0.0
+        if (y != NAN):
+            self.traj_req.y = y
+        else:
+            self.traj_req.y = 0.0
+        if (z != NAN):
+            self.traj_req.z = z
+        else:
+            self.traj_req.z = 0.0
         self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
         self.future = self.traj_cli.call_async(self.traj_req)
         rclpy.spin_until_future_complete(self, self.future)
@@ -86,8 +99,20 @@ class TestController(Node):
 
     def send_position_request(self, x, y, z, angle):
         self.traj_req.control_mode = 3
-        self.traj_req.yaw = angle
-        self.traj_req.values = [x, y, z]
+        if (angle != NAN):
+            self.traj_req.yaw = angle
+        if (x != NAN):
+            self.traj_req.x = x
+        else:
+            self.traj_req.x = 0.0
+        if (y != NAN):
+            self.traj_req.y = y
+        else:
+            self.traj_req.y = 0.0
+        if (z != NAN):
+            self.traj_req.z = z
+        else:
+            self.traj_req.z = 0.0
         self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle))
         self.future = self.traj_cli.call_async(self.traj_req)
         rclpy.spin_until_future_complete(self, self.future)
@@ -104,99 +129,99 @@ class TestController(Node):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=0.0, thrust=0.05)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=0.1, angle=0.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=1.0, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=0.5, angle=0.0)
+            self.send_position_request(x=NAN, y=NAN, z=1.0, angle=NAN)
 
     def move_right(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=1.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0)
+            self.send_velocity_request(x=1.0, y=NAN, z=NAN, angle=NAN)
         else:
-            self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0)
+            self.send_position_request(x=1.0, y=NAN, z=NAN, angle=NAN)
 
     def move_down(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=0.0, thrust=-0.05)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=-0.1, angle=0.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=-0.5, angle=0.0)
+            self.send_position_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
 
     def move_left(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=-1.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0)
+            self.send_velocity_request(x=-1.0, y=NAN, z=NAN, angle=NAN)
         else:
-            self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0)
+            self.send_position_request(x=-1.0, y=NAN, z=NAN, angle=NAN)
 
     def rotate_right(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=1.0,
                                        roll=0.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=NAN, angle=1.0)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0)
+            self.send_position_request(x=NAN, y=NAN, z=NAN, angle=1.0)
 
     def rotate_left(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=-1.0,
                                        roll=0.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=NAN, angle=-1.0)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0)
+            self.send_position_request(x=NAN, y=NAN, z=NAN, angle=-1.0)
 
     def move_forward(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=-1.0, yaw=0.0,
                                        roll=0.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0)
+            self.send_velocity_request(x=NAN, y=1.0, z=NAN, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0)
+            self.send_position_request(x=NAN, y=1.0, z=NAN, angle=NAN)
 
     def move_backward(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=1.0, yaw=0.0,
                                        roll=0.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0)
+            self.send_velocity_request(x=NAN, y=-1.0, z=NAN, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0)
+            self.send_position_request(x=NAN, y=-1.0, z=NAN, angle=NAN)
 
     def stop(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=0.0, thrust=0.0)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0)
+            self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0)
+            self.send_position_request(x=0.0, y=0.0, z=0.0, angle=NAN)
 
     def move_up_little(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=0.0, thrust=0.01)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=0.05, angle=0.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=0.5, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0)
+            self.send_position_request(x=NAN, y=NAN, z=1.0, angle=NAN)
 
     def move_down_little(self):
         if (self.control_mode == CONTROL_MODE_ATTITUDE):
             self.send_attitude_request(pitch=0.0, yaw=0.0,
                                        roll=0.0, thrust=-0.01)
         elif (self.control_mode == CONTROL_MODE_VELOCITY):
-            self.send_velocity_request(x=0.0, y=0.0, z=-0.05, angle=0.0)
+            self.send_velocity_request(x=NAN, y=NAN, z=-0.5, angle=NAN)
         else:
-            self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0)
+            self.send_position_request(x=NAN, y=NAN, z=-1.0, angle=NAN)
 
     def on_press(self, key):
         try: