Skip to content

Commit

Permalink
Revert pieces of filter viz
Browse files Browse the repository at this point in the history
  • Loading branch information
@jpanikulam
jpanikulam committed Mar 30, 2019
1 parent f8239e3 commit 40bbd90
Show file tree
Hide file tree
Showing 3 changed files with 59 additions and 30 deletions.
2 changes: 1 addition & 1 deletion control/quadraframe_model.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ namespace control {
struct QuadraframeConfiguration {
const jcc::Vec3 quadraframe_from_vane_unit_0_translation = jcc::Vec3(0.0, 46.7e-3, -9.5e-3);

SE3 com_from_vane_unit_0 = SE3(jcc::exp_z(M_PI).so3(), quadraframe_from_vane_unit_0_translation);
SE3 com_from_vane_unit_0 = jcc::exp_z(0.5 * M_PI) * SE3(SO3(), quadraframe_from_vane_unit_0_translation);
SE3 com_from_vane_unit_1 = jcc::exp_z(M_PI * 0.5) * com_from_vane_unit_0;
SE3 com_from_vane_unit_2 = jcc::exp_z(M_PI * 0.5) * com_from_vane_unit_1;
SE3 com_from_vane_unit_3 = jcc::exp_z(M_PI * 0.5) * com_from_vane_unit_2;
Expand Down
80 changes: 53 additions & 27 deletions filtering/filter_viz_balsaq.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,13 @@
#include "filtering/filter_viz_balsaq.hh"

#include "config/fiducial_map/read_fiducial_map.hh"
#include "control/servo_interface.hh"
#include "embedded/imu_driver/imu_message.hh"
#include "embedded/servo_bq/set_servo_message.hh"
#include "filtering/pose_message.hh"
#include "infrastructure/balsa_queue/bq_main_macro.hh"
#include "vision/fiducial_detection_message.hh"
#include "visualization/thrust_stand_visualizer.hh"

#include <cstddef>
#include <iostream>
Expand Down Expand Up @@ -39,8 +42,9 @@ void FilterVizBq::init(int argc, char* argv[]) {
background->add_plane({ground, 0.1});
background->flip();

geo_ = view->add_primitive<viewer::SimpleGeometry>();
persistent_ = view->add_primitive<viewer::SimpleGeometry>();
sensor_geo_ = view->add_primitive<viewer::SimpleGeometry>();
pose_geo_ = view->add_primitive<viewer::SimpleGeometry>();
servo_geo_ = view->add_primitive<viewer::SimpleGeometry>();

std::cout << "Subscribing IMU" << std::endl;
imu_sub_ = make_subscriber("imu");
Expand Down Expand Up @@ -113,16 +117,17 @@ void FilterVizBq::draw_sensors() {
accels.reserve(accel_history_.size());
for (const auto& meas : accel_history_) {
accels.push_back(meas.accel_mpss);
geo_->add_point({meas.accel_mpss, jcc::Vec4(0.1, 0.7, 0.3, 0.9)});
sensor_geo_->add_point({meas.accel_mpss, jcc::Vec4(0.1, 0.7, 0.3, 0.9)});
}

if (accel_history_.size() > 2500u) {
const auto visitor = [this, &view](const geometry::shapes::EllipseFit& fit) {
geo_->add_ellipsoid({fit.ellipse, jcc::Vec4(0.4, 0.6, 0.4, 0.7), 2.0});
geo_->flush();
sensor_geo_->add_ellipsoid({fit.ellipse, jcc::Vec4(0.4, 0.6, 0.4, 0.7), 2.0});
sensor_geo_->flush();
};
// std::cout << "Optimizing" << std::endl;
const auto result = geometry::shapes::fit_ellipse(accels);
geo_->add_ellipsoid({result.ellipse, jcc::Vec4(0.2, 0.9, 0.2, 1.0), 5.0});
sensor_geo_->add_ellipsoid({result.ellipse, jcc::Vec4(0.2, 0.9, 0.2, 1.0), 5.0});

std::cerr << "chol" << std::endl;
std::cerr << result.ellipse.cholesky_factor << std::endl;
Expand All @@ -138,40 +143,43 @@ void FilterVizBq::draw_sensors() {
mag_utesla_.pop_front();
}

geo_->add_sphere({tag_from_world_.inverse().translation(), 0.3});
geo_->add_axes({tag_from_world_.inverse()});
sensor_geo_->add_sphere({tag_from_world_.inverse().translation(), 0.3});
sensor_geo_->add_axes({tag_from_world_.inverse()});

geo_->add_sphere({camera_from_body_.inverse().translation(), 0.3});
geo_->add_axes({camera_from_body_.inverse()});
sensor_geo_->add_sphere({camera_from_body_.inverse().translation(), 0.3});
sensor_geo_->add_axes({camera_from_body_.inverse()});

if (!fiducial_history_.empty()) {
const SE3 fiducial_from_camera = fiducial_history_.back();

// geo_->add_axes({fiducial_from_camera, 0.025, 3.0});
constexpr bool DRAW_FIDUCIAL_POSE = true;
constexpr bool DRAW_VEHICLE_POSE = false;
constexpr bool DRAW_FIDUCIAL_IN_BODY_FRAME = false;

{ // Draw the fiducial axes in the camera frame
geo_->add_sphere({fiducial_from_camera.inverse().translation(), 0.2, jcc::Vec4(1.0, 1.0, 0.2, 0.8)});
geo_->add_axes({fiducial_from_camera.inverse(), 0.025, 3.0});
if (DRAW_FIDUCIAL_POSE) { // Draw the fiducial axes in the camera frame
sensor_geo_->add_sphere({fiducial_from_camera.inverse().translation(), 0.2, jcc::Vec4(1.0, 1.0, 0.2, 0.8)});
sensor_geo_->add_axes({fiducial_from_camera.inverse(), 0.025, 3.0});
}

{ // Draw the vehicle axes in the world frame
if (DRAW_VEHICLE_POSE) { // Draw the vehicle axes in the world frame
const SE3 world_from_vehicle = tag_from_world_.inverse() * fiducial_from_camera * camera_from_body_;
geo_->add_sphere({world_from_vehicle.translation(), 0.2, jcc::Vec4(1.0, 0.0, 0.2, 0.8)});
geo_->add_axes({world_from_vehicle, 0.6, 3.0, true});
sensor_geo_->add_sphere({world_from_vehicle.translation(), 0.2, jcc::Vec4(1.0, 0.0, 0.2, 0.8)});
sensor_geo_->add_axes({world_from_vehicle, 0.6, 3.0, true});
}

{ // Draw the body axes in the fiducial frame
if (DRAW_FIDUCIAL_IN_BODY_FRAME) { // Draw the body axes in the fiducial frame
const SE3 fiducial_from_body = fiducial_from_camera * camera_from_body_;
geo_->add_axes({fiducial_from_body.inverse()});
geo_->add_sphere({fiducial_from_body.inverse().translation(), 0.2, jcc::Vec4(0.0, 0.0, 1.0, 0.8)});
sensor_geo_->add_axes({fiducial_from_body.inverse()});
sensor_geo_->add_sphere({fiducial_from_body.inverse().translation(), 0.2, jcc::Vec4(0.0, 0.0, 1.0, 0.8)});
}
}

if (!accel_history_.empty()) {
geo_->add_line({jcc::Vec3::Zero(), accel_history_.back().accel_mpss});
sensor_geo_->add_line({jcc::Vec3::Zero(), accel_history_.back().accel_mpss});
}

geo_->add_sphere({jcc::Vec3::Zero(), 9.81});
sensor_geo_->add_sphere({jcc::Vec3::Zero(), 9.81});
sensor_geo_->flip();
}

void FilterVizBq::draw_pose() {
Expand All @@ -183,17 +191,35 @@ void FilterVizBq::draw_pose() {

if (got_pose_msg) {
const Pose pose = pose_msg.to_pose();
std::cout << "Got pose message" << std::endl;
geo_->add_axes({pose.world_from_jet, 0.055, 3.0, true});
pose_geo_->add_axes({pose.world_from_jet, 0.055, 3.0, true});
pose_geo_->flip();
}
}

void FilterVizBq::draw_servos() {
const control::VaneConfiguration vane_cfg = {};
const control::QuadraframeConfiguration qframe_cfg = {};

SetServoMessage servo_message;
bool got_servo_msg = false;
while (servo_sub_->read(servo_message, 1)) {
got_servo_msg = true;
}
if (got_servo_msg) {
const control::QuadraframeStatus qframe_status = control::create_quadraframe_status(servo_message);
visualization::put_quadraframe(*servo_geo_, qframe_status, qframe_cfg, vane_cfg);
std::cout << "--" << std::endl;
std::cout << qframe_status.servo_0_angle_rad << ", " << qframe_status.servo_1_angle_rad << ", "
<< qframe_status.servo_2_angle_rad << ", " << qframe_status.servo_3_angle_rad << std::endl;

servo_geo_->flip();
}
}

void FilterVizBq::loop() {
draw_sensors();
draw_pose();
// draw_servos();

geo_->flip();
draw_servos();
}

void FilterVizBq::shutdown() {
Expand Down
7 changes: 5 additions & 2 deletions filtering/filter_viz_balsaq.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ class FilterVizBq : public BalsaQ {

void draw_sensors();
void draw_pose();
void draw_servos();

// Every millisecond
const static uint loop_delay_microseconds = 10000;
Expand All @@ -37,14 +38,16 @@ class FilterVizBq : public BalsaQ {
SE3 tag_from_world_;
SE3 camera_from_body_;

std::shared_ptr<viewer::SimpleGeometry> geo_;
std::shared_ptr<viewer::SimpleGeometry> persistent_;
std::shared_ptr<viewer::SimpleGeometry> sensor_geo_;
std::shared_ptr<viewer::SimpleGeometry> pose_geo_;
std::shared_ptr<viewer::SimpleGeometry> servo_geo_;

struct AccelStuff {
jcc::Vec3 accel_mpss;
jcc::Vec3 gyro_radps;
jcc::Vec3 mag_utesla;
};

std::deque<AccelStuff> accel_history_;
std::deque<SE3> fiducial_history_;
std::deque<jcc::Vec3> mag_utesla_;
Expand Down

0 comments on commit 40bbd90

Please sign in to comment.