0.2.5 (#57)

0.2.5, final release before moving to 16.04 and Kinetic

README.md

@@ -45,9 +45,16 @@ Please submit pull requests to the [devel branch](https://github.com/AutoRally/a
    * synaptic
    * arduino
    * python-termcolor
+
+3. __Install MPPI Dependencies (only if you have a GPU and will run MPPI)__
+
+    The core idea behind MPPI is to sample thousands of trajectories really fast. This is accomplished by implementing the sampling step on a GPU, for which you will need CUDA. We also use an external library to load python's numpy zip archives (.npz files) into C++.
+
+    * [Install CUDA](https://developer.nvidia.com/cuda-downloads)
+    * [Install CNPY](https://github.com/rogersce/cnpy)
 
-3. __[Install](http://www.ros.org/install/) ros-indigo-desktop-full__
-4. __Install gtsam__
+4. __[Install](http://www.ros.org/install/) ros-indigo-desktop-full__
+5. __Install gtsam__
 
    Follow the gtsam [Quick Start](https://bitbucket.org/gtborg/gtsam/) guide to clone and install the _develop_ branch of gtsam. 
 
@@ -146,6 +153,7 @@ More detailed explanations of the controllers and state estimator can be found o
 * [State estimator](https://github.com/AutoRally/autorally/wiki/State%20Estimator)
 * [Waypoint follower](https://github.com/AutoRally/autorally/wiki/Waypoint%20Following)
 * [Constant speed controller](https://github.com/AutoRally/autorally/wiki/Constant%20Speed)
+* [MPPI Controller](https://github.com/AutoRally/autorally/wiki/Model-Predictive-Path-Integral-Controller-(MPPI))
 
 [Controlling the AutoRally platform](https://github.com/AutoRally/autorally/wiki/Controlling%20the%20AutoRally%20Platform) is a tutorial for how your own controller can control the AutoRally platform (in simulation or on hardware).
 

autorally/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package autorally
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.2.5 (2017-07-03)
+------------------
+
 0.2.4 (2017-02-14)
 ------------------
 

autorally/package.xml

@@ -16,7 +16,7 @@
 -->
 <package>
   <name>autorally</name>
-  <version>0.2.4</version>
+  <version>0.2.5</version>
   <description>Autorally metapackage</description>
 
   <author>Brian Goldfain</author>

autorally_control/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package autorally_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.2.5 (2017-07-03)
+------------------
+* Initial public MPPI release, tutorial is available on wiki
+* A few minor bug fixes and updates for c++11
+
 0.2.4 (2017-02-14)
 ------------------
 

autorally_control/CMakeLists.txt

@@ -16,24 +16,28 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 find_package(Boost 1.49.0 REQUIRED)
-find_package(Eigen3 REQUIRED)
+find_package(Eigen REQUIRED)
 
 set(BUILD_FLAGS "-std=c++11 -Wuninitialized")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${BUILD_FLAGS}")
 
-generate_dynamic_reconfigure_options(cfg/gpsWaypoint_params.cfg)
+generate_dynamic_reconfigure_options(cfg/gpsWaypoint_params.cfg
+				     cfg/PathIntegralParams.cfg)
 
 catkin_package(
   DEPENDS Boost
   CATKIN-DEPENDS roscpp std_msgs geometry_msgs nav_msgs nodelet dynamic_reconfigure autorally_core autorally_msgs
   INCLUDE_DIRS include
+  LIBRARIES autorally_plant param_getter
+
 )
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
 
 add_subdirectory(src/ConstantSpeedController)
 add_subdirectory(src/joystick)
 add_subdirectory(src/gpsWaypoint)
+add_subdirectory(src/path_integral)
 
 install(DIRECTORY launch/
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch

autorally_control/cfg/PathIntegralParams.cfg

@@ -0,0 +1,24 @@
+#! /usr/bin/env python
+
+PACKAGE='autorally_control'
+import roslib
+roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name       Type      Level Description     Default Min   Max
+
+gen.add("max_throttle", double_t, 0, "Maximum applied throttle", 0.5, 0.0, 1.0)
+gen.add("desired_speed", double_t, 0, "Speed Target for the MPPI controller", 6.0, 0.0, 25.0)
+gen.add("speed_coefficient", double_t, 0, "Weight for acheiving target velocity", 4.25, 0.0, 20.0)
+gen.add("track_coefficient", double_t, 0, "Weight for staying on the track", 100.0, 0, 500.0)
+gen.add("max_slip_angle", double_t, 0, "Maximum slip angle allowed", .75, 0.0, 3.14)
+gen.add("slip_penalty", double_t, 0, "Penalty for violating slip angle threshold", 500.0, 0, 1000.0)
+gen.add("crash_coefficient", double_t, 0, "Penalty for crashing", 10000, 0, 20000)
+gen.add("track_slop", double_t, 0, "Value for clipping track cost to zero.", 0, 0, .75)
+gen.add("steering_coeff", double_t, 0, "Steering Cost Coefficient", .1, 0, 1.0)
+gen.add("throttle_coeff", double_t, 0, "Throttle Cost Coefficient", .1, 0, 1.0)
+
+exit(gen.generate(PACKAGE, "PathIntegral", "PathIntegralParams"))

autorally_control/include/autorally_control/path_integral/autorally_plant.h

@@ -0,0 +1,208 @@
+/*
+* Software License Agreement (BSD License)
+* Copyright (c) 2013, Georgia Institute of Technology
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright notice, this
+* list of conditions and the following disclaimer.
+* 2. 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.
+*
+* 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 OWNER 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 autorally_plant.h
+ * @author Grady Williams <gradyrw@gmail.com>
+ * @date May 24, 2017
+ * @copyright 2017 Georgia Institute of Technology
+ * @brief Class definition for AutorallyPlant class.
+ ***********************************************/
+
+#ifndef AUTORALLY_PLANT_H_
+#define AUTORALLY_PLANT_H_
+
+#include <ros/ros.h>
+
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Point.h>
+
+#include <autorally_msgs/chassisCommand.h>
+#include <autorally_msgs/chassisState.h>
+#include <autorally_msgs/runstop.h>
+#include <autorally_msgs/pathIntegralStatus.h>
+
+#include <eigen3/Eigen/Dense>
+
+namespace autorally_control {
+
+/**
+* @class AutorallyPlant autorally_plant.h
+* @brief Publishers and subscribers for the autorally control system.
+* 
+* This class is treated as the plant for the MPPI controller. When the MPPI
+* controller has a control it sends to a function in this class to get
+* send to the actuators. Likewise it calls functions in this class to receive
+* state feedback. This class also publishes trajectory and spray information
+* and status information for both the controller and the OCS.
+*/
+
+class AutorallyPlant //: public Diagnostics  
+{
+public:
+  static const int AUTORALLY_STATE_DIM = 7;
+  //Struct for holding the autorally pose.
+  typedef struct
+  { 
+    //X-Y-Z position
+    float x_pos;
+    float y_pos;
+    float z_pos;
+    //1-2-3 Euler angles
+    float roll;
+    float pitch;
+    float yaw;
+    //Quaternions
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    //X-Y-Z velocity.
+    float x_vel;
+    float y_vel;
+    float z_vel;
+    //Body frame velocity
+    float u_x;
+    float u_y;
+    //Euler angle derivatives
+    float yaw_mder;
+    //Current servo commands
+    float steering;
+    float throttle;
+  } FullState;
+
+  bool new_model_available_;
+
+  /**
+  * @brief Constructor for AutorallyPlant, takes the a ros node handle and initalizes
+  * publishers and subscribers.
+  * @param mppi_node A ros node handle.
+  */
+	AutorallyPlant(ros::NodeHandle mppi_node, bool debug_mode, int hz);
+
+  /**
+  * @brief Destructor for AutorallyPlant.
+  */
+	~AutorallyPlant();
+
+  /**
+  * @brief Callback for /pose_estimate subscriber.
+  */
+	void poseCall(nav_msgs::Odometry pose_msg);
+
+  /**
+  * @brief Callback for recording the current servo input.
+  */
+  void servoCall(autorally_msgs::chassisState servo_msg);
+
+  /**
+  * @brief Callback for safe speed subscriber.
+  */
+	void runstopCall(autorally_msgs::runstop safe_msg);
+
+  /**
+  * @brief Publishes the controller's nominal path.
+  */
+	void pubPath(float* nominal_traj, int num_timesteps, int hz);
+
+  /**
+  * @brief Publishes a control input. 
+  * @param steering The steering command to publish.
+  * @param throttle The throttle command to publish.
+  */
+	void pubControl(float steering, float throttle);
+
+  void pubStatus();
+
+  /**
+  * @brief Returns the current state of the system.
+  */
+	FullState getState();
+
+  /**
+  * @brief Returns whether or not the state updater has gone stale or not.
+  */
+  bool getStale();
+
+  /**
+  * @brief Returns the current value of safe speed.
+  */
+  bool getRunstop();
+
+  /**
+  * @brief Returns the timestamp of the last pose callback.
+  */
+  ros::Time getLastPoseTime();
+
+  /**
+  * @brief Checks the system status.
+  * @return An integer specifying the status. 0 means the system is operating
+  * nominally, 1 means something is wrong but no action needs to be taken,
+  * 2 means that the vehicle should stop immediately.
+  */
+  int checkStatus();
+
+
+  std::vector<float> getModelParams();
+
+private:
+  const double TIMEOUT = 0.5; ///< Time before declaring pose/controls stale. 
+
+  FullState full_state_; ///< Full state of the autorally vehicle.
+
+  int hz_; ///< The frequency of the control publisher.
+
+  int status_; ///< System status
+  std::string ocs_msg_; ///< Message to send to the ocs.
+
+  bool safe_speed_zero_; ///< Current value of safe speed.
+  bool debug_mode_; ///< Whether or not the system is in debug/simulation mode.
+  bool activated_; ///< Whether or not we've received an initial pose message.
+
+  ros::Time last_check_; //Timestamp of the last published control.
+  ros::Time last_pose_call_; ///< Timestamp of the last pose callback.
+
+  ros::Publisher control_pub_; ///< Publisher of autorally_msgs::chassisCommand type on topic servoCommand.
+  ros::Publisher path_pub_; ///< Publisher of nav_mags::Path on topic nominalPath.
+  ros::Publisher status_pub_; ///< Publishes the status (0 good, 1 neutral, 2 bad) of the controller
+  ros::Publisher delay_pub_; ///< Publisher of geometry::msgs::Point on topic mppiTimeDelay.
+  ros::Subscriber pose_sub_; ///< Subscriber to /pose_estimate.
+  ros::Subscriber servo_sub_;
+
+  autorally_msgs::chassisCommand control_msg_; ///< Autorally control message initialization.
+  nav_msgs::Path path_msg_; ///< Path message for publishing the planned path.
+  geometry_msgs::Point time_delay_msg_; ///< Point message for publishing the observed delay.
+  autorally_msgs::pathIntegralStatus status_msg_; ///<pathIntegralStatus message for publishing mppi status
+  std::vector<float> model_params_; ///< Array for holding the updated model parameters
+
+};
+
+}
+
+#endif /* AUTORALLY_PLANT_H_ */