|
| 1 | +/* |
| 2 | +* Software License Agreement (BSD License) |
| 3 | +* Copyright (c) 2013, Georgia Institute of Technology |
| 4 | +* All rights reserved. |
| 5 | +* |
| 6 | +* Redistribution and use in source and binary forms, with or without |
| 7 | +* modification, are permitted provided that the following conditions are met: |
| 8 | +* |
| 9 | +* 1. Redistributions of source code must retain the above copyright notice, this |
| 10 | +* list of conditions and the following disclaimer. |
| 11 | +* 2. Redistributions in binary form must reproduce the above copyright notice, |
| 12 | +* this list of conditions and the following disclaimer in the documentation |
| 13 | +* and/or other materials provided with the distribution. |
| 14 | +* |
| 15 | +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 16 | +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 17 | +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 18 | +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE |
| 19 | +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| 20 | +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
| 21 | +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 22 | +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
| 23 | +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| 24 | +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 25 | +*/ |
| 26 | +/********************************************** |
| 27 | + * @file autorally_plant.h |
| 28 | + * @author Grady Williams <gradyrw@gmail.com> |
| 29 | + * @date May 24, 2017 |
| 30 | + * @copyright 2017 Georgia Institute of Technology |
| 31 | + * @brief Class definition for AutorallyPlant class. |
| 32 | + ***********************************************/ |
| 33 | + |
| 34 | +#ifndef AUTORALLY_PLANT_H_ |
| 35 | +#define AUTORALLY_PLANT_H_ |
| 36 | + |
| 37 | +#include <ros/ros.h> |
| 38 | + |
| 39 | +#include <nav_msgs/Odometry.h> |
| 40 | +#include <nav_msgs/Path.h> |
| 41 | + |
| 42 | +#include <geometry_msgs/Quaternion.h> |
| 43 | +#include <geometry_msgs/PoseStamped.h> |
| 44 | +#include <geometry_msgs/Point.h> |
| 45 | + |
| 46 | +#include <autorally_msgs/chassisCommand.h> |
| 47 | +#include <autorally_msgs/chassisState.h> |
| 48 | +#include <autorally_msgs/runstop.h> |
| 49 | +#include <autorally_msgs/pathIntegralStatus.h> |
| 50 | + |
| 51 | +#include <eigen3/Eigen/Dense> |
| 52 | + |
| 53 | +namespace autorally_control { |
| 54 | + |
| 55 | +/** |
| 56 | +* @class AutorallyPlant autorally_plant.h |
| 57 | +* @brief Publishers and subscribers for the autorally control system. |
| 58 | +* |
| 59 | +* This class is treated as the plant for the MPPI controller. When the MPPI |
| 60 | +* controller has a control it sends to a function in this class to get |
| 61 | +* send to the actuators. Likewise it calls functions in this class to receive |
| 62 | +* state feedback. This class also publishes trajectory and spray information |
| 63 | +* and status information for both the controller and the OCS. |
| 64 | +*/ |
| 65 | + |
| 66 | +class AutorallyPlant //: public Diagnostics |
| 67 | +{ |
| 68 | +public: |
| 69 | + static const int AUTORALLY_STATE_DIM = 7; |
| 70 | + //Struct for holding the autorally pose. |
| 71 | + typedef struct |
| 72 | + { |
| 73 | + //X-Y-Z position |
| 74 | + float x_pos; |
| 75 | + float y_pos; |
| 76 | + float z_pos; |
| 77 | + //1-2-3 Euler angles |
| 78 | + float roll; |
| 79 | + float pitch; |
| 80 | + float yaw; |
| 81 | + //Quaternions |
| 82 | + float q0; |
| 83 | + float q1; |
| 84 | + float q2; |
| 85 | + float q3; |
| 86 | + //X-Y-Z velocity. |
| 87 | + float x_vel; |
| 88 | + float y_vel; |
| 89 | + float z_vel; |
| 90 | + //Body frame velocity |
| 91 | + float u_x; |
| 92 | + float u_y; |
| 93 | + //Euler angle derivatives |
| 94 | + float yaw_mder; |
| 95 | + //Current servo commands |
| 96 | + float steering; |
| 97 | + float throttle; |
| 98 | + } FullState; |
| 99 | + |
| 100 | + bool new_model_available_; |
| 101 | + |
| 102 | + /** |
| 103 | + * @brief Constructor for AutorallyPlant, takes the a ros node handle and initalizes |
| 104 | + * publishers and subscribers. |
| 105 | + * @param mppi_node A ros node handle. |
| 106 | + */ |
| 107 | + AutorallyPlant(ros::NodeHandle mppi_node, bool debug_mode, int hz); |
| 108 | + |
| 109 | + /** |
| 110 | + * @brief Destructor for AutorallyPlant. |
| 111 | + */ |
| 112 | + ~AutorallyPlant(); |
| 113 | + |
| 114 | + /** |
| 115 | + * @brief Callback for /pose_estimate subscriber. |
| 116 | + */ |
| 117 | + void poseCall(nav_msgs::Odometry pose_msg); |
| 118 | + |
| 119 | + /** |
| 120 | + * @brief Callback for recording the current servo input. |
| 121 | + */ |
| 122 | + void servoCall(autorally_msgs::chassisState servo_msg); |
| 123 | + |
| 124 | + /** |
| 125 | + * @brief Callback for safe speed subscriber. |
| 126 | + */ |
| 127 | + void runstopCall(autorally_msgs::runstop safe_msg); |
| 128 | + |
| 129 | + /** |
| 130 | + * @brief Publishes the controller's nominal path. |
| 131 | + */ |
| 132 | + void pubPath(float* nominal_traj, int num_timesteps, int hz); |
| 133 | + |
| 134 | + /** |
| 135 | + * @brief Publishes a control input. |
| 136 | + * @param steering The steering command to publish. |
| 137 | + * @param throttle The throttle command to publish. |
| 138 | + */ |
| 139 | + void pubControl(float steering, float throttle); |
| 140 | + |
| 141 | + void pubStatus(); |
| 142 | + |
| 143 | + /** |
| 144 | + * @brief Returns the current state of the system. |
| 145 | + */ |
| 146 | + FullState getState(); |
| 147 | + |
| 148 | + /** |
| 149 | + * @brief Returns whether or not the state updater has gone stale or not. |
| 150 | + */ |
| 151 | + bool getStale(); |
| 152 | + |
| 153 | + /** |
| 154 | + * @brief Returns the current value of safe speed. |
| 155 | + */ |
| 156 | + bool getRunstop(); |
| 157 | + |
| 158 | + /** |
| 159 | + * @brief Returns the timestamp of the last pose callback. |
| 160 | + */ |
| 161 | + ros::Time getLastPoseTime(); |
| 162 | + |
| 163 | + /** |
| 164 | + * @brief Checks the system status. |
| 165 | + * @return An integer specifying the status. 0 means the system is operating |
| 166 | + * nominally, 1 means something is wrong but no action needs to be taken, |
| 167 | + * 2 means that the vehicle should stop immediately. |
| 168 | + */ |
| 169 | + int checkStatus(); |
| 170 | + |
| 171 | + |
| 172 | + std::vector<float> getModelParams(); |
| 173 | + |
| 174 | +private: |
| 175 | + const double TIMEOUT = 0.5; ///< Time before declaring pose/controls stale. |
| 176 | + |
| 177 | + FullState full_state_; ///< Full state of the autorally vehicle. |
| 178 | + |
| 179 | + int hz_; ///< The frequency of the control publisher. |
| 180 | + |
| 181 | + int status_; ///< System status |
| 182 | + std::string ocs_msg_; ///< Message to send to the ocs. |
| 183 | + |
| 184 | + bool safe_speed_zero_; ///< Current value of safe speed. |
| 185 | + bool debug_mode_; ///< Whether or not the system is in debug/simulation mode. |
| 186 | + bool activated_; ///< Whether or not we've received an initial pose message. |
| 187 | + |
| 188 | + ros::Time last_check_; //Timestamp of the last published control. |
| 189 | + ros::Time last_pose_call_; ///< Timestamp of the last pose callback. |
| 190 | + |
| 191 | + ros::Publisher control_pub_; ///< Publisher of autorally_msgs::chassisCommand type on topic servoCommand. |
| 192 | + ros::Publisher path_pub_; ///< Publisher of nav_mags::Path on topic nominalPath. |
| 193 | + ros::Publisher status_pub_; ///< Publishes the status (0 good, 1 neutral, 2 bad) of the controller |
| 194 | + ros::Publisher delay_pub_; ///< Publisher of geometry::msgs::Point on topic mppiTimeDelay. |
| 195 | + ros::Subscriber pose_sub_; ///< Subscriber to /pose_estimate. |
| 196 | + ros::Subscriber servo_sub_; |
| 197 | + |
| 198 | + autorally_msgs::chassisCommand control_msg_; ///< Autorally control message initialization. |
| 199 | + nav_msgs::Path path_msg_; ///< Path message for publishing the planned path. |
| 200 | + geometry_msgs::Point time_delay_msg_; ///< Point message for publishing the observed delay. |
| 201 | + autorally_msgs::pathIntegralStatus status_msg_; ///<pathIntegralStatus message for publishing mppi status |
| 202 | + std::vector<float> model_params_; ///< Array for holding the updated model parameters |
| 203 | + |
| 204 | +}; |
| 205 | + |
| 206 | +} |
| 207 | + |
| 208 | +#endif /* AUTORALLY_PLANT_H_ */ |
0 commit comments