Add Square Scrimmage Plugin and Initialization Maneuver for testing Visual Odometry Algorithms

include/scrimmage/plugins/autonomy/Square/Square.h

@@ -0,0 +1,110 @@
+/*!
+ * @file
+ *
+ * @section LICENSE
+ *
+ * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI)
+ *
+ * This file is part of SCRIMMAGE.
+ *
+ *   SCRIMMAGE is free software: you can redistribute it and/or modify it under
+ *   the terms of the GNU Lesser General Public License as published by the
+ *   Free Software Foundation, either version 3 of the License, or (at your
+ *   option) any later version.
+ *
+ *   SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT
+ *   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ *   License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with SCRIMMAGE.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Natalie Rakoski <[email protected]>
+ * @author Kevin DeMarco <[email protected]>
+ * @date 10 April 2021
+ * @version 0.1.0
+ * @brief Brief file description.
+ * @section DESCRIPTION
+ * A Long description goes here.
+ *
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_
+#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/entity/Contact.h>
+#include <scrimmage/math/Quaternion.h>
+
+#include <Eigen/Dense>
+
+#include <map>
+#include <string>
+#include <memory>
+
+namespace scrimmage {
+
+//namespace interaction {
+//class BoundaryBase;
+//}
+
+namespace autonomy {
+
+class InitManeuverType {
+ public:
+    bool active = false;
+    Quaternion init_man_goal_quat;
+    bool stay_straight = false;
+};
+
+class Square : public scrimmage::Autonomy{
+ public:
+    void init(std::map<std::string, std::string> &params) override;
+    bool step_autonomy(double t, double dt) override;
+
+ protected:
+    double speed_;
+    Eigen::Vector3d goal_;
+    Eigen::Vector3d next_goal_;
+    Eigen::Vector3d goal1_;
+    Eigen::Vector3d goal2_;
+    Eigen::Vector3d goal3_;
+    Eigen::Vector3d goal4_;
+
+    int frame_number_;
+    bool show_camera_images_;
+    bool save_camera_images_;
+
+    int desired_alt_idx_ = 0;
+    int desired_speed_idx_ = 0;
+    int desired_heading_idx_ = 0;
+
+    scrimmage_proto::ShapePtr text_shape_;
+    scrimmage_proto::ShapePtr sphere_shape_;
+
+    bool noisy_state_set_ = false;
+    State noisy_state_;
+
+    ContactMap noisy_contacts_;
+
+    double desired_z_ = 0;
+
+    PublisherPtr init_maneuver_pub_;
+    bool use_init_maneuver_ = false;
+    bool init_maneuver_finished_ = true;
+    std::deque<State> get_init_maneuver_positions();
+    std::deque<State> man_positions_;
+    Eigen::Vector3d init_man_goal_pos_;
+    Quaternion init_man_goal_quat_;
+
+    int square_side_ = 1;
+    double sq_side_length_m_ = 100;
+    double start_corner_turn_ = 10;
+    Eigen::Vector3d init_goal_;
+    // StatePtr init_state_;
+    Eigen::Vector3d prev_diff_ = {0,0,0};
+
+};
+} // namespace autonomy
+} // namespace scrimmage
+#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_SQUARE_SQUARE_H_

include/scrimmage/plugins/autonomy/Square/Square.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
+<params>
+  <library>Square_plugin</library>
+  <speed>21</speed>
+  <show_camera_images>false</show_camera_images>
+  <save_camera_images>false</save_camera_images>
+<!--  <enable_boundary_control>false</enable_boundary_control>-->
+<!--  <generate_entities>false</generate_entities>-->
+  <use_init_maneuver>false</use_init_maneuver>
+  <sq_side_length_m>100</sq_side_length_m>
+  <!-- specify how many meters before the corner would you like the vehicle to start turning -->
+  <start_corner_turn>10</start_corner_turn>
+</params>

include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.h

@@ -0,0 +1,98 @@
+/*!
+ * @file
+ *
+ * @section LICENSE
+ *
+ * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI)
+ *
+ * This file is part of SCRIMMAGE.
+ *
+ *   SCRIMMAGE is free software: you can redistribute it and/or modify it under
+ *   the terms of the GNU Lesser General Public License as published by the
+ *   Free Software Foundation, either version 3 of the License, or (at your
+ *   option) any later version.
+ *
+ *   SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT
+ *   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ *   License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with SCRIMMAGE.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Kevin DeMarco <[email protected]>
+ * @author Eric Squires <[email protected]>
+ * @date 31 July 2017
+ * @version 0.1.0
+ * @brief Brief file description.
+ * @section DESCRIPTION
+ * A Long description goes here.
+ *
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_
+#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/entity/Contact.h>
+
+#include <Eigen/Dense>
+
+#include <map>
+#include <string>
+#include <memory>
+
+namespace scrimmage {
+
+namespace interaction {
+class BoundaryBase;
+}
+
+namespace autonomy {
+
+class InitManeuverType {
+ public:
+    bool active = false;
+    Quaternion init_man_goal_quat;
+    bool stay_straight = false;
+};
+
+class StraightVOInit : public scrimmage::Autonomy{
+ public:
+    void init(std::map<std::string, std::string> &params) override;
+    bool step_autonomy(double t, double dt) override;
+
+ protected:
+    double speed_;
+    Eigen::Vector3d goal_;
+    Eigen::Vector3d init_goal_;
+
+    int frame_number_;
+    bool show_camera_images_;
+    bool save_camera_images_;
+    bool show_text_label_;
+
+    bool enable_boundary_control_ = false;
+    std::shared_ptr<scrimmage::interaction::BoundaryBase> boundary_;
+
+    int desired_alt_idx_ = 0;
+    int desired_speed_idx_ = 0;
+    int desired_heading_idx_ = 0;
+
+    bool noisy_state_set_ = false;
+    State noisy_state_;
+    ContactMap noisy_contacts_;
+
+    double desired_z_ = 0;
+    Eigen::Vector3d prev_diff_ = {0,0,0};
+
+    PublisherPtr init_maneuver_pub_;
+    bool use_init_maneuver_ = false;
+    bool init_maneuver_finished_ = true;
+    std::deque<State> get_init_maneuver_positions();
+    std::deque<State> man_positions_;
+    Eigen::Vector3d init_man_goal_pos_;
+    Quaternion init_man_goal_quat_;
+};
+} // namespace autonomy
+} // namespace scrimmage
+#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_STRAIGHTVOINIT_STRAIGHTVOINIT_H_

include/scrimmage/plugins/autonomy/StraightVOInit/StraightVOInit.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
+<params>
+  <library>StraightVOInit_plugin</library>
+  <speed>21</speed>
+  <show_camera_images>false</show_camera_images>
+  <save_camera_images>false</save_camera_images>
+  <enable_boundary_control>false</enable_boundary_control>
+
+  <use_init_maneuver>false</use_init_maneuver>
+
+</params>

include/scrimmage/plugins/sensor/AirSimSensor/AirSimSensor.h

@@ -33,7 +33,9 @@
 #define INCLUDE_SCRIMMAGE_PLUGINS_SENSOR_AIRSIMSENSOR_AIRSIMSENSOR_H_
 
 #include <scrimmage/sensor/Sensor.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/Quaternion.h>
 #include <scrimmage/common/CSV.h>
 
 #include <random>
@@ -132,7 +134,7 @@ class AirSimSensor : public scrimmage::Sensor {
 
  protected:
     std::string vehicle_name_ = "none";
-    bool save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, StatePtr& state, int frame_num);
+    bool save_data(MessagePtr<std::vector<AirSimImageType>>& im_msg, State state, int frame_num);
     scrimmage::CSV csv;
     int airsim_frame_num_ = 0;
 
@@ -189,6 +191,12 @@ class AirSimSensor : public scrimmage::Sensor {
     bool get_image_data_ = true;
     bool get_lidar_data_ = true;
     bool get_imu_data_ = true;
+
+    // Init Maneuver
+    bool init_maneuver_active_ = false;
+    Quaternion init_man_orig_quat_;
+    bool stay_straight_ = false;
+
     // period at which the data acquisition is run [seconds]
     // double data_acquisition_period_ = .1;
     double image_acquisition_period_ = .1;

include/scrimmage/plugins/sensor/ROSAltimeter/ROSAltimeter.h

@@ -35,6 +35,8 @@
 #include <ros/ros.h>
 #include <mavros_msgs/Altitude.h>
 #include <scrimmage/sensor/Sensor.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/common/CSV.h>
 
 #include <random>
 #include <vector>
@@ -52,13 +54,16 @@ class ROSAltimeter : public scrimmage::Sensor {
     ROSAltimeter();
     void init(std::map<std::string, std::string> &params) override;
     bool step() override;
+    void close(double t) override;
 
  protected:
     std::string vehicle_name_ = "none";
     std::string ros_namespace_;
     std::shared_ptr<ros::NodeHandle> nh_;
     ros::Publisher altimeter_pub_;
     float monotonic_ = 0.0;
+    double prev_time_ = 0.0;
+    scrimmage::CSV csv;
 
  private:
 };

include/scrimmage/plugins/sensor/ROSCompass/ROSCompass.h

@@ -35,6 +35,8 @@
 #include <ros/ros.h>
 #include <sensor_msgs/MagneticField.h>
 #include <scrimmage/sensor/Sensor.h>
+#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/common/CSV.h>
 
 #include <random>
 #include <vector>
@@ -50,12 +52,15 @@ class ROSCompass : public scrimmage::Sensor {
     ROSCompass();
     void init(std::map<std::string, std::string> &params) override;
     bool step() override;
+    void close(double t) override;
 
  protected:
     std::string vehicle_name_ = "none";
     std::string ros_namespace_;
     std::shared_ptr<ros::NodeHandle> nh_;
     ros::Publisher compass_pub_;
+    double prev_time_ = 0.0;
+    scrimmage::CSV csv;
 
  private:
 };