C++ Examples
monoDrive C++ Client Examples
The monoDrive C++ Client comes with examples for connecting to the monoDrive
Simulator or Scenario Editor and controlling the ego vehicle in both Replay and
Closed Loop modes. The examples can be found in the examples/cpp
directory in the monodive-client
repo.
Connecting to the Simulator
To connect to a running instance of the monoDrive Simulator or Scenario Editor
in "Play" mode, read in configuration values and create an instance of a
Simulator
object:
//Read JSON files in cpp_client/config directory
Configuration config(
"examples/config/simulator.json",
"examples/config/weather.json",
"examples/config/scenario.json"
);
// Default simulator port is 8999
Simulator& sim0 = Simulator::getInstance(config, "127.0.0.1", 8999);
if (!sim0.configure())
{
return -1;
}
Creating Sensors
Each sensor has a configuration struct
that must be setup either by reading in
a JSON configuration file or by creating a configuration object and assigning the appropriate values. For instance, constructing a camera sensor should look like:
// Read the JSON from file
std::ifstream in("/path/to/my/config.json");
nlohmann::json file;
file << in;
// Import into the camera config struct
CameraConfig camera_config;
camera_config.from_json(file);
Alternatively, the struct
can be setup directly in the code:
// define desired sensors
std::vector<std::shared_ptr<Sensor>> sensors;
CameraConfig fc_config;
fc_config.server_ip = sim0.getServerIp();
fc_config.server_port = sim0.getServerPort();
fc_config.listen_port = 8100;
fc_config.location.z = 225;
fc_config.rotation.pitch = -5;
fc_config.resolution = Resolution(1920,1080);
fc_config.annotation.include_annotation = true;
fc_config.annotation.desired_tags = {"traffic_sign"};
After the sensor's configuration is created, all that needs to be done is configure the sensor:
sensors.push_back(std::make_shared<Sensor>(std::make_unique<CameraConfig>(fc_config)));
for (auto& sensor : sensors)
{
sensor->configure();
}
Stepping in Replay Mode
After the simulator
object is connected and the sensors are configured, the
simulation can be stepped in replay mode:
for(; idx < nSteps; idx++)
{
//step simulator
task = std::async([&simulator, &idx](){
return simulator.step(idx, 1);
});
After each step, a sensor sample can be acquired using the sensor's sample()
function:
//sample all sensors
for(auto& sensor : sensors)
{
sensor.sample();
}
if(!task.get()){
break;
}
} // for
Controlling the ego vehicle
As an alternative to Replay mode, a closed loop control over the ego vehicle can be done by sending control commands to the simulator, here the vehicle is requested to move forward with 50% throttle:
simulator.send_command(ApiMessage(123, EgoControl_ID, true,
{ {"forward_amount", 0.5},
{"right_amount", 0.0},
{"brake_amount", 0.0},
{"drive_mode", 1}
}));
Replay Example
A full example of controlling the ego vehicle in Replay mode can be found in
monodrive-client/examples/cpp/replay/replay.cpp
. This
example demonstrates:
- Configuring and connecting to a running instance of the monoDrive Simulator
- Configuring and connecting to a Camera sensor (as discussed above)
- Configuring the Viewport Sensor for the camera on the simulator
- Stepping a monoDrive Trajectory File in Replay mode (as discussed above)
The Viewport Sensor in this example controls where the camera view will be
placed in the monoDrive Simulator or Scenario Editor. This sensor is not
configured and sent to the sim0
instance without needing to be sampled:
ViewportCameraConfig vp_config;
vp_config.server_ip = sim0.getServerIp();
vp_config.server_port = sim0.getServerPort();
vp_config.location.z = 200;
vp_config.resolution = Resolution(256,256);
Sensor(std::make_unique<ViewportCameraConfig>(vp_config)).configure();
Lane Follower Examples
Two examples of controlling the ego vehicle in a closed loop mode can be found in
the Lane Follower example provided in
monodrive-client/examples/cpp/lane_follower/
. These examples demonstrate:
- Configuring and connecting to a running instance of the monoDrive Simulator
- Configuring and connecting to a Camera sensor (as discussed above)
- Configuring the Viewport Sensor for the camera on the simulator (as discussed in the Replay Example)
- Configuring and connecting a State Sensor sensor to get state information from the ego vehicle
- Issuing control commands to the simulator to keep the ego vehicle within a lane
The State Sensor information is used to stream ego vehicle state information back to the client:
StateConfig state_config;
state_config.desired_tags = {"ego"};
state_config.server_ip = sim0.getServerIp();
state_config.server_port = sim0.getServerPort();
state_config.listen_port = 8101;
state_config.debug_drawing = true;
state_config.undesired_tags = {""};
sensors.push_back(std::make_shared<Sensor>(std::make_unique<StateConfig>(state_config)));
These examples query the simulator for the OpenDrive Map definition, parse it using the client's map API, and query the resulting data structure to determine the target location for the ego vehicle. The map query to the simulator is sent and read here:
MapConfig map_request;
map_request.format = "opendrive";
ApiMessage response;
sim0.sendCommand(map_request.message(), &response);
map_data = carla::opendrive::OpenDriveParser::Load(
nlohmann::json::parse(response.get_message().get<std::string>()).at("map")
);
To issue vehicle control commands for keeping the ego vehicle within its current lane, first grab the vehicle information from the state sensor
EgoControlConfig planning(DataFrame* dataFrame){
auto& frame = *static_cast<StateFrame*>(dataFrame);
VehicleState* vehicle_frame = nullptr;
for(auto& vehicle : frame.vehicles){
for(auto& tag : vehicle.state.tags){
if(tag == "ego"){
vehicle_frame = &vehicle;
break;
}
}
}
if(vehicle_frame == nullptr){
std::cout << "No ego vehicle in frame." << std::endl;
return EgoControlConfig();
}
Eigen::VectorXd position(3);
position << vehicle_frame->state.odometry.pose.position.x,
vehicle_frame->state.odometry.pose.position.y,
vehicle_frame->state.odometry.pose.position.z;
Eigen::Quaternion<double> orientation(
vehicle_frame->state.odometry.pose.orientation.w,
vehicle_frame->state.odometry.pose.orientation.x,
vehicle_frame->state.odometry.pose.orientation.y,
vehicle_frame->state.odometry.pose.orientation.z
);
}
Now compute the vehicle's current distance from the lane and steer the vehicle towards the correct position:
auto currentWp = map_data->GetClosestWaypointOnRoad(carla::geom::Location(
float(position[0] / 100.0),
float(position[1] / 100.0),
float(position[2] / 100.0)));
if(currentWp.has_value()) {
auto nextWps = map_data->GetNext(*currentWp, 6.0);
auto currentPos = map_data->ComputeTransform(*currentWp).location;
if(nextWps.size() > 0) {
auto nextLocation = map_data->ComputeTransform(nextWps[0]).location;
nextPoint = Eigen::Vector3d(nextLocation.x, nextLocation.y, nextLocation.z)*100.0;
}
}
Eigen::VectorXd dirToNextPoint = nextPoint - position;
dirToNextPoint.normalize();
double angle = -dirToNextPoint.head<3>().cross(forwardVector.head<3>())[2];
// use PID speed controller for throttle
Eigen::VectorXd velocity(3);
velocity << vehicle_frame->state.odometry.linear_velocity.x,
vehicle_frame->state.odometry.linear_velocity.y,
vehicle_frame->state.odometry.linear_velocity.z;
double speed = velocity.norm();
double dt = last_time > 0 ? frame.game_time - last_time : 0.1;
last_time = frame.game_time;
float throttle = last_throttle;
if (dt) {
throttle = pid.pid(float(desired_speed - speed), float(dt));
last_throttle = throttle;
}
// form controls response
EgoControlConfig egoControl;
egoControl.forward_amount = std::max(0.0f, throttle);
egoControl.brake_amount = std::min(0.0f, throttle);
egoControl.drive_mode = 1;
egoControl.right_amount = (float)angle;
return egoControl;
}
Finally, create the new control command to the vehicle and send it:
sim0.sendCommand(ApiMessage(777, EgoControl_ID, true, egoControl.dump()));
}
The Fixed Step Example, monodrive-client/examples/cpp/lane_follower/fixed_step.cpp
, contains a fixed amount of time between each step of the simulation physics in order to allow slow perception algorithms to keep up.
The Real Time Example, monodrive-client/examples/cpp/lane_follower/real_time.cpp
, enables the simulation physics to go as fast as they can.