Flightmare Integration
Flightmare is composed of two main components: a configurable rendering engine built on Unity and a flexible physics engine for dynamics simulation. Those two components are totally decoupled and can run independently from each other. Hence, we can combine Flightmare with Agilicious for Hardware-in-the-loop (HITL) simulation. HITL simulation allows for flying a physical quadrotor in the real-world while observing virtual photorealistic environments.
Download Flightmare
Clone the code:
git clone git@github.com:uzh-rpg/flightmare.git
Ignore the Flightmare ROS examples:
touch flightmare/flightros/CATKIN_IGNORE
Download Flightmare Unity Standalone:
curl --show-error --progress-bar --location "https://github.com/uzh-rpg/flightmare/releases/download/0.0.5/RPG_Flightmare.tar.xz" | tar Jxf - -C flightmare/flightrender/ --strip 1
Hardware-in-the-loop Simulation
Now, assume you have both Agilicious and Flightmare downloaded. You need to create a new ros package that combines both. We provides some hints on how to use Unity rendering here. For details, you can check this example.
Set up Unity rendering. Example code:
// Flightmare Quadrotor and Unity Camera
unity_quad_ = std::make_shared<flightlib::Quadrotor>();
flightlib::Vector<3> quad_size(0.5, 0.5, 0.2);
unity_quad_->setSize(quad_size);
unity_camera_ = std::make_shared<flightlib::RGBCamera>();
flightlib::Vector<3> B_r_BC(0.0, 0.0, 0.3);
Scalar pitch_angle_deg = 0.0;
flightlib::Matrix<3, 3> R_BC =
(Eigen::AngleAxisf(0.0 * M_PI, Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(-pitch_angle_deg / 180.0 * M_PI,
Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(-0.5 * M_PI, Eigen::Vector3f::UnitZ()))
.toRotationMatrix();
double hor_fov_radians = (M_PI * (110 / 180.0));
double width = 640.0;
double height = 480.0;
// Recalculate here: https://themetalmuncher.github.io/fov-calc/;
double vertical_fov =
2. * std::atan(std::tan(hor_fov_radians / 2) * height / width);
vertical_fov = (vertical_fov / M_PI) * 180.0; // convert back to degrees
std::cout << "Vertical FoV is " << vertical_fov << std::endl;
unity_camera_->setFOV(vertical_fov);
unity_camera_->setWidth(width);
unity_camera_->setHeight(height);
unity_camera_->setRelPose(B_r_BC, R_BC);
unity_quad_->addRGBCamera(unity_camera_);
// Connect Unity
setUnity(unity_render_);
connectUnity();
Set Unity:
bool setUnity(const bool render) {
unity_render_ = render;
if (unity_render_ && unity_bridge_ == nullptr) {
unity_bridge_ = flightlib::UnityBridge::getInstance();
unity_bridge_->addQuadrotor(unity_quad_);
//
if (!loadRacetrack(pnh_)) {
ROS_WARN("[%s] No race track is specified.", pnh_.getNamespace().c_str());
} else {
};
ROS_INFO("[%s] Unity Bridge is created.", pnh_.getNamespace().c_str());
return true;
} else {
return false;
}
}
Connect Unity:
bool connectUnity() {
if (!unity_render_ || unity_bridge_ == nullptr) return false;
unity_ready_ = unity_bridge_->connectUnity(unity_scene_id_);
return unity_ready_;
}
The images are rendered based the current quadrotor states. Below are example codes for publishing images. Publish images:
void publishImages(const QuadState &state) {
sensor_msgs::ImagePtr rgb_msg;
frame_id_ += 1;
// render the frame
flightlib::QuadState unity_quad_state;
unity_quad_state.setZero();
unity_quad_state.p = state.p.cast<flightlib::Scalar>();
unity_quad_state.qx = state.qx.cast<flightlib::Scalar>();
unity_quad_->setState(unity_quad_state);
// Warning, delay
unity_bridge_->getRender(frame_id_);
unity_bridge_->handleOutput(frame_id_);
cv::Mat img;
unity_camera_->getRGBImage(img);
rgb_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
rgb_msg->header.stamp = ros::Time(state.t);
image_pub_.publish(rgb_msg);
}