This module hosts the RHC controller first implemented on MuSHR stack. It is a model predictive contoller that plans to waypoints from a goal (instead of a reference trajectory). This controller is suitable for cars that don't have a planning module, but want simple MPC. There are the following modules:
mushr_rhc
, hosts the RHC library, doesn't depend on ROS.mushr_rhc_ros
, hooks up the library with ROS.mushr_rhc_mujoco
, hooks up the library withmushr_mujoco_py
, a set of python bindings in mushr_mujoco_ros
Before using the library, you should install it locally using pip
:
$ cd ~/catkin_ws/src/mushr_rhc # or wherever you cloned the repo
$ pip install -e .
This will allow you to use the library anywhere the system.
Note: if you are using the mushr image you can just clone the repo into ~/catkin_ws/src
and it should work out of the box
Get pip:
sudo apt install python-pip
To run this module on the car, you need a few packages. To get them download the wheel file for torch from nvidia:
$ wget https://nvidia.box.com/shared/static/m6vy0c7rs8t1alrt9dqf7yt1z587d1jk.whl -O torch-1.1.0a0+b457266-cp27-cp27mu-linux_aarch64.whl
$ pip install torch-1.1.0a0+b457266-cp27-cp27mu-linux_aarch64.whl
Then get the future package:
pip install future
Then get the python packages necessary:
$ sudo apt install python-scipy
$ sudo apt install python-networkx
$ sudo apt install python-sklearn
librhc
(mushr_rhc_ros/src/librhc
) is the core MPC code, with the other source being ROS interfacing code. The main components are:
- Cost function (
librhc/cost
): Takes into account the cost-to-go, collisions and other information to produce a cost for a set of trajectories. - Model (
librhc/model
): A model of the car, currenly using the kinematic bicycle model. - Trajectory generation (
librhc/trajgen
): Strategies for generating trajectory libraries for MPC to evaluate. - Value function (
librhc/value
): Evaluation of positions of the car with resepct to a goal. - World Representation (
librhc/workrep
): An occupancy grid based representation for the map.
Topic | Type | Description |
---|---|---|
/rhcontroller/markers |
visualization_msgs/Marker | Halton points sampled in the map (for debugging purposes). |
/rhcontroller/traj_chosen |
geometry_msgs/PoseArray | The lowest cost trajectory (for debuggin purposes). |
/mux/ackermann_cmd_mux/input/navigation |
ackermann_msgs/AckermannDriveStamped | The lowest cost control to apply on the car. |
Topic | Type | Description |
---|---|---|
/map_metadata |
nav_msgs/MapMetaData | Uses dimension and resolution to create occupancy grid. |
/move_base_simple/goal |
geometry_msgs/PoseStamped | Goal to compute path to. |
/car_pose |
geometry_msgs/PoseStamped | When using simulated car pose Current pose of the car. |
/pf/inferred_pose |
geometry_msgs/PoseStamped | When using particle filter for localization Current pose of the car. |
Topic | Type | Description |
---|---|---|
/rhcontroller/reset/hard |
std_srvs/Empty | Creates a new instance of the MPC object, redoing all initialization computation. |
/rhcontroller/reset/soft |
std_srvs/Empty | Resets parameters only, not redoing initialization. |