|
|
|
# Register a new component
|
|
|
|
|
|
|
|
Once you have created a raw c++ component inside the `crs` directory, you will need to register it as a ros component inside the `ros` directory. While in theory, all code could directly go into the `ros` section, we still advise to create two different packages detaching ROS functionality from raw algorithmic implementations.
|
|
|
|
|
|
|
|
## Registering a new Simulator
|
|
|
|
If you would like to register a new simulator, you will first need to create a new class that inherits from the abstract `Simulator` class defined in `ros/ros_simulator/include/ros_simulator/common/ros_simulator.h`. Have a look at `ros/ros_simulator/src/ros_pacejka_simulator.cpp`to see an example implementation.
|
|
|
|
|
|
|
|
Once the ros component is created, edit the file `ros/ros_simulator/src/component_registry.cpp` to enable your component to be loaded.
|
|
|
|
|
|
|
|
```c++
|
|
|
|
if (state_type == "your_state_type" && input_type == "your_input_type")
|
|
|
|
{
|
|
|
|
auto* your_simulator = new your_simulator ()
|
|
|
|
for (auto sensor_name : sensors_to_load)
|
|
|
|
{
|
|
|
|
// Register sensor in simulator
|
|
|
|
}
|
|
|
|
return your_simulator;
|
|
|
|
}
|
|
|
|
```
|
|
|
|
## Registering a new Estimator
|
|
|
|
If you would like to register a new estimator, you will first need to create a new class that inherits from the abstract `RosStateEstimator` class defined in `ros/ros_estimators/include/ros_estimators/state_estimator_ros.h`. Have a look at `src/ros/ros_estimators/src/lowpass_estimator/pacejka_lowpass.cpp`to see an example implementation using a lowpass filter.
|
|
|
|
|
|
|
|
|
|
|
|
Once the ros component is created, edit the file `ros/ros_estimators/src/component_registry.cpp` to enable your component to be loaded.
|
|
|
|
|
|
|
|
```c++
|
|
|
|
if (estimator_type == "your_estimator_type")
|
|
|
|
{
|
|
|
|
return new your_ros_estimator(nh, nh_private);
|
|
|
|
}
|
|
|
|
```
|
|
|
|
|
|
|
|
## Registering a new Controller
|
|
|
|
If you would like to register a new controller, that implements from the class `BaseController` you will only need to register it in the `ros/ros_controllers/src/component_registry.cpp` file.
|
|
|
|
|
|
|
|
```c++
|
|
|
|
inline RosController<...>* getYourController(ros::NodeHandle& nh, ros::NodeHandle& nh_private, void*& dynamic_callback_allocator)
|
|
|
|
{
|
|
|
|
// Create controller
|
|
|
|
auto ptr = std::make_shared<YOUR_CONTROLLER>(...);
|
|
|
|
// Downcast to BaseController type
|
|
|
|
auto derived_ptr = std::dynamic_pointer_cast<crs_controls::BaseController<your_state, your_input>>(ptr);
|
|
|
|
// Create Visualizer
|
|
|
|
auto visualizer_ptr = loadControllerVisualizer<void, pacejka_state, pacejka_input>(
|
|
|
|
ros::NodeHandle(nh_private, "visualizer"), derived_ptr);
|
|
|
|
// Optional, if Dynamic Reconfigure is implemented
|
|
|
|
dynamic_callback_allocator = (void*) new YourDynamicReconfigureServer(ptr);
|
|
|
|
return new pacejka_ros_controller(nh, nh_private, std::move(visualizer_ptr), derived_ptr);
|
|
|
|
}
|
|
|
|
|
|
|
|
template <>
|
|
|
|
pacejka_ros_controller* resolveController<ros_car_state, ros_car_input, your_state, your_type>( ... )
|
|
|
|
{
|
|
|
|
if (controller_type == "your_controller_type")
|
|
|
|
{
|
|
|
|
return getYourController(nh, nh_private, dynamic_callback_allocator);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
``` |