Merge pull request #21 from aerostack2/platform_test

[as2_platform_pixhawk] Platform test

Author: miferco97

config/platform_config_file.yaml

@@ -2,7 +2,6 @@
   ros__parameters:
     cmd_freq: 100.0  # Hz of platform commands send
     info_freq: 10.0  # Hz of platform info publish
-    mass: 1.0  # Kg
-    max_thrust: 15.0  # N
-    min_thrust: 0.15  # N
-    external_odom: false  # Use external odometry source
+    max_thrust: 15.0  # Max thrust to send to platform (N)
+    min_thrust: 0.15  # Min thrust to send to platform (N)
+    external_odom: false  # Flag to use external odometry source

include/as2_platform_pixhawk/pixhawk_platform.hpp

@@ -58,26 +58,30 @@
 #include <px4_msgs/msg/vehicle_odometry.hpp>
 #include <px4_msgs/msg/vehicle_rates_setpoint.hpp>
 
-#include <as2_core/utils/frame_utils.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/battery_state.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <sensor_msgs/msg/nav_sat_status.hpp>
+#include "as2_core/custom/tf2_geometry_msgs.hpp"
+
+#include "as2_core/utils/frame_utils.hpp"
 #include "as2_core/aerial_platform.hpp"
 #include "as2_core/names/topics.hpp"
 #include "as2_core/sensor.hpp"
 #include "as2_core/utils/tf_utils.hpp"
 #include "as2_msgs/msg/control_mode.hpp"
 #include "as2_msgs/msg/thrust.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "sensor_msgs/msg/battery_state.hpp"
-#include "sensor_msgs/msg/imu.hpp"
-#include "sensor_msgs/msg/nav_sat_fix.hpp"
-#include "sensor_msgs/msg/nav_sat_status.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+namespace as2_platform_pixhawk
+{
 
 class PixhawkPlatform : public as2::AerialPlatform
 {
 public:
-  PixhawkPlatform();
+  explicit PixhawkPlatform(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   ~PixhawkPlatform() {}
 
 public:
@@ -155,7 +159,6 @@ class PixhawkPlatform : public as2::AerialPlatform
   px4_msgs::msg::VehicleRatesSetpoint px4_rates_setpoint_;
   px4_msgs::msg::VehicleOdometry px4_visual_odometry_msg_;
 
-  float mass_;
   float max_thrust_;
   float min_thrust_;
   bool simulation_mode_ = false;
@@ -177,4 +180,6 @@ class PixhawkPlatform : public as2::AerialPlatform
   void px4GpsCallback(const px4_msgs::msg::SensorGps::SharedPtr msg);
 };
 
+}  // namespace as2_platform_pixhawk
+
 #endif  // AS2_PLATFORM_PIXHAWK__PIXHAWK_PLATFORM_HPP_

src/pixhawk_platform.cpp

@@ -39,17 +39,17 @@
 
 #include "pixhawk_platform.hpp"
 
-PixhawkPlatform::PixhawkPlatform()
-: as2::AerialPlatform()
+namespace as2_platform_pixhawk
+{
+
+PixhawkPlatform::PixhawkPlatform(const rclcpp::NodeOptions & options)
+: as2::AerialPlatform(options)
 {
   configureSensors();
 
   base_link_frame_id_ = as2::tf::generateTfName(this, "base_link");
   odom_frame_id_ = as2::tf::generateTfName(this, "odom");
 
-  this->declare_parameter<float>("mass");
-  mass_ = this->get_parameter("mass").as_double();
-
   this->declare_parameter<float>("max_thrust");
   max_thrust_ = this->get_parameter("max_thrust").as_double();
 
@@ -59,7 +59,6 @@ PixhawkPlatform::PixhawkPlatform()
   this->declare_parameter<bool>("external_odom");
   external_odom_ = this->get_parameter("external_odom").as_bool();
 
-  RCLCPP_INFO(this->get_logger(), "Mass: %f", mass_);
   RCLCPP_INFO(this->get_logger(), "Max thrust: %f", max_thrust_);
   RCLCPP_INFO(this->get_logger(), "Min thrust: %f", min_thrust_);
   RCLCPP_INFO(
@@ -835,3 +834,5 @@ bool PixhawkPlatform::getFlagSimulationMode()
   // TODO(miferco97): check if this is better than creating a variable to store the value
   return this->get_parameter("use_sim_time").as_bool();
 }
+
+}  // namespace as2_platform_pixhawk

src/pixhawk_platform_node.cpp

@@ -43,7 +43,7 @@
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  auto node = std::make_shared<PixhawkPlatform>();
+  auto node = std::make_shared<as2_platform_pixhawk::PixhawkPlatform>();
   node->preset_loop_frequency(300);
   as2::spinLoop(node);
 

tests/pixhawk_platform_test.cpp

@@ -0,0 +1,277 @@
+// Copyright 2023 Universidad Politécnica de Madrid
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Universidad Politécnica de Madrid nor the names
+//    of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/**
+ * @file pixhawk_platform_test.cpp
+ *
+ * Pixhawk test
+ *
+ * @author Rafael Perez-Segui <[email protected]>
+ */
+
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include "pixhawk_platform.hpp"
+#include "as2_core/mocks/aerial_platform/mock_aerial_platform.hpp"
+#include "as2_core/mocks/executor_thread_util/executor_thread_util.hpp"
+#include <as2_msgs/msg/platform_state_machine_event.hpp>
+
+namespace as2_platform_pixhawk
+{
+std::shared_ptr<PixhawkPlatform> get_node(
+  const std::string & name_space = "test_pixhawk_platform")
+{
+  const std::string package_path =
+    ament_index_cpp::get_package_share_directory("as2_platform_pixhawk");
+  const std::string control_modes_config_file = package_path + "/config/control_modes.yaml";
+  const std::string platform_config_file = package_path + "/config/platform_config_file.yaml";
+
+  std::vector<std::string> node_args = {
+    "--ros-args",
+    "-r",
+    "__ns:=/" + name_space,
+    "-p",
+    "namespace:=" + name_space,
+    "-p",
+    "control_modes_file:=" + control_modes_config_file,
+    "--params-file",
+    platform_config_file,
+  };
+
+  rclcpp::NodeOptions node_options;
+  node_options.arguments(node_args);
+
+  return std::make_shared<PixhawkPlatform>(node_options);
+}
+
+void spinForTime(
+  double seconds, rclcpp::executors::SingleThreadedExecutor & executor,
+  std::shared_ptr<as2::mock::PlatformMockNode> test_node)
+{
+  // Current ros2 time
+  auto start_time = test_node->getTime();
+  while ((test_node->getTime() - start_time).seconds() < seconds) {
+    // Spin
+    executor.spin_some();
+
+    // Print state
+    // test_node->printState(seconds * 0.5);
+
+    // Sleep for 10 ms
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  }
+  return;
+}
+
+void spinForTime(
+  double seconds,
+  std::shared_ptr<as2::mock::PlatformMockNode> test_node)
+{
+  // Current ros2 time
+  auto start_time = test_node->getTime();
+  while ((test_node->getTime() - start_time).seconds() < seconds) {
+    // Print state
+    // test_node->printState(seconds * 0.5);
+
+    // Sleep for 10 ms
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  }
+  return;
+}
+
+int platform_test()
+{
+  std::string ros_namespace = "test_pixhawk_platform";
+  auto test_node = std::make_shared<as2::mock::PlatformMockNode>(ros_namespace);
+  auto tello_node = get_node(ros_namespace);
+
+  // Executor
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor =
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  // Add nodes to executor
+  executor->add_node(test_node);
+  executor->add_node(tello_node);
+
+  // Executor thread
+  as2::mock::ExecutorThreadUtil executor_thread_util(executor, 200.0);
+  executor_thread_util.start();
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+  // Enable test command send timer
+  test_node->setCommandSendTimerState(true, 100.0);
+
+  std::string base_frame = as2::tf::generateTfName(
+    test_node->get_namespace(), "base_link");
+
+  // Wait 2 seconds
+  spinForTime(2.0, test_node);
+
+  // Arm
+  if (!test_node->setArmingState(true, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Set offboard
+  if (!test_node->setOffboardControl(true, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Send transition event TAKE_OFF
+  as2_msgs::msg::PlatformStateMachineEvent takeoff_event;
+  takeoff_event.event = as2_msgs::msg::PlatformStateMachineEvent::TAKE_OFF;
+  if (!test_node->setPlatformStateMachineEvent(takeoff_event, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Set takeoff_twist actuator command
+  geometry_msgs::msg::TwistStamped takeoff_twist;
+  takeoff_twist.header.frame_id = base_frame;
+  takeoff_twist.twist.linear.x = 0.0;
+  takeoff_twist.twist.linear.y = 0.0;
+  takeoff_twist.twist.linear.z = 0.5;
+  takeoff_twist.twist.angular.x = 0.0;
+  takeoff_twist.twist.angular.y = 0.0;
+  takeoff_twist.twist.angular.z = 0.0;
+  test_node->setCommandTwist(takeoff_twist);
+  spinForTime(0.5, test_node);
+
+  // Set control mode to speed for takeoff
+  as2_msgs::msg::ControlMode control_mode;
+  control_mode.control_mode = as2_msgs::msg::ControlMode::SPEED;
+  control_mode.yaw_mode = as2_msgs::msg::ControlMode::YAW_SPEED;
+  control_mode.reference_frame = as2_msgs::msg::ControlMode::LOCAL_ENU_FRAME;
+  if (!test_node->setControlMode(control_mode, false)) {
+    return -1;
+  }
+  spinForTime(5.0, test_node);
+
+  // Send transition event TOOK_OFF
+  as2_msgs::msg::PlatformStateMachineEvent tookoff_event;
+  tookoff_event.event = as2_msgs::msg::PlatformStateMachineEvent::TOOK_OFF;
+  if (!test_node->setPlatformStateMachineEvent(tookoff_event, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Hover
+  geometry_msgs::msg::TwistStamped hover_twist;
+  hover_twist.header.frame_id = base_frame;
+  hover_twist.twist.linear.x = 0.0;
+  hover_twist.twist.linear.y = 0.0;
+  hover_twist.twist.linear.z = 0.0;
+  hover_twist.twist.angular.x = 0.0;
+  hover_twist.twist.angular.y = 0.0;
+  hover_twist.twist.angular.z = 0.0;
+  test_node->setCommandTwist(hover_twist);
+  spinForTime(5.0, test_node);
+
+  // Move forward
+  geometry_msgs::msg::TwistStamped forward_twist;
+  forward_twist.header.frame_id = as2::tf::generateTfName(
+    test_node->get_namespace(), base_frame);
+  forward_twist.twist.linear.x = 0.5;
+  forward_twist.twist.linear.y = 0.0;
+  forward_twist.twist.linear.z = 0.0;
+  forward_twist.twist.angular.x = 0.0;
+  forward_twist.twist.angular.y = 0.0;
+  forward_twist.twist.angular.z = 0.0;
+  test_node->setCommandTwist(forward_twist);
+  spinForTime(5.0, test_node);
+
+  // Move backward
+  geometry_msgs::msg::TwistStamped backward_twist;
+  backward_twist.header.frame_id = base_frame;
+  backward_twist.twist.linear.x = -0.5;
+  backward_twist.twist.linear.y = 0.0;
+  backward_twist.twist.linear.z = 0.0;
+  backward_twist.twist.angular.x = 0.0;
+  backward_twist.twist.angular.y = 0.0;
+  backward_twist.twist.angular.z = 0.0;
+  test_node->setCommandTwist(backward_twist);
+  spinForTime(5.0, test_node);
+
+  // Send transition event LAND
+  as2_msgs::msg::PlatformStateMachineEvent land_event;
+  land_event.event = as2_msgs::msg::PlatformStateMachineEvent::LAND;
+  if (!test_node->setPlatformStateMachineEvent(land_event, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Set land_twist actuator command
+  geometry_msgs::msg::TwistStamped land_twist;
+  land_twist.header.frame_id = base_frame;
+  land_twist.twist.linear.x = 0.0;
+  land_twist.twist.linear.y = 0.0;
+  land_twist.twist.linear.z = -0.5;
+  land_twist.twist.angular.x = 0.0;
+  land_twist.twist.angular.y = 0.0;
+  land_twist.twist.angular.z = 0.0;
+  test_node->setCommandTwist(land_twist);
+  spinForTime(10.0, test_node);
+
+  // Send transition event LANDED
+  as2_msgs::msg::PlatformStateMachineEvent landed_event;
+  landed_event.event = as2_msgs::msg::PlatformStateMachineEvent::LANDED;
+  if (!test_node->setPlatformStateMachineEvent(landed_event, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Send disarm command
+  if (!test_node->setArmingState(false, false)) {
+    return -1;
+  }
+  spinForTime(0.5, test_node);
+
+  // Clean
+  executor_thread_util.stop();
+  executor->remove_node(test_node);
+  executor->remove_node(tello_node);
+
+  return 0;
+}
+}  // namespace as2_platform_pixhawk
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto result = as2_platform_pixhawk::platform_test();
+
+  rclcpp::shutdown();
+  return result;
+}