diff --git a/control/quadraframe_model.hh b/control/quadraframe_model.hh index f930f7d..0342e62 100644 --- a/control/quadraframe_model.hh +++ b/control/quadraframe_model.hh @@ -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; diff --git a/filtering/filter_viz_balsaq.cc b/filtering/filter_viz_balsaq.cc index cdd27b6..8b52d94 100644 --- a/filtering/filter_viz_balsaq.cc +++ b/filtering/filter_viz_balsaq.cc @@ -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 #include @@ -39,8 +42,9 @@ void FilterVizBq::init(int argc, char* argv[]) { background->add_plane({ground, 0.1}); background->flip(); - geo_ = view->add_primitive(); - persistent_ = view->add_primitive(); + sensor_geo_ = view->add_primitive(); + pose_geo_ = view->add_primitive(); + servo_geo_ = view->add_primitive(); std::cout << "Subscribing IMU" << std::endl; imu_sub_ = make_subscriber("imu"); @@ -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; @@ -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() { @@ -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() { diff --git a/filtering/filter_viz_balsaq.hh b/filtering/filter_viz_balsaq.hh index 81f56b6..a3e40a7 100644 --- a/filtering/filter_viz_balsaq.hh +++ b/filtering/filter_viz_balsaq.hh @@ -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; @@ -37,14 +38,16 @@ class FilterVizBq : public BalsaQ { SE3 tag_from_world_; SE3 camera_from_body_; - std::shared_ptr geo_; - std::shared_ptr persistent_; + std::shared_ptr sensor_geo_; + std::shared_ptr pose_geo_; + std::shared_ptr servo_geo_; struct AccelStuff { jcc::Vec3 accel_mpss; jcc::Vec3 gyro_radps; jcc::Vec3 mag_utesla; }; + std::deque accel_history_; std::deque fiducial_history_; std::deque mag_utesla_;