fix (hack) a problem in quaternion issue using QuadStateEstiamte

Peter010103 · Sep 1, 2020 · 12a90eb · 12a90eb
1 parent adb07d9
commit 12a90eb
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 6 deletions.
diff --git a/flightlib/src/common/math.cpp b/flightlib/src/common/math.cpp
@@ -1,4 +1,5 @@
 #include "flightlib/common/math.hpp"
+#include "iostream"
 
 namespace flightlib {
 

diff --git a/flightros/include/flightros/flight_pilot.hpp b/flightros/include/flightros/flight_pilot.hpp
@@ -65,7 +65,6 @@ class FlightPilot {
   uint16_t receive_id_{0};
 
   // auxiliary variables
-  quadrotor_common::QuadStateEstimate state_est_;
   Scalar main_loop_freq_{50.0};
 };
 }  // namespace flightros
diff --git a/flightros/launch/rotors_gazebo.launch b/flightros/launch/rotors_gazebo.launch
@@ -6,7 +6,7 @@
   <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
   <arg name="world_name" default="$(find rotors_gazebo)/worlds/basic.world"/>
 
-  <arg name="use_unity_editor" value="false" />
+  <arg name="use_unity_editor" default="true" />
   <arg name="paused" value="true"/>
   <arg name="gui" value="true"/>
   <arg name="use_mpc" default="false"/>

diff --git a/flightros/src/flight_pilot.cpp b/flightros/src/flight_pilot.cpp
@@ -56,10 +56,14 @@ FlightPilot::FlightPilot(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
 FlightPilot::~FlightPilot() {}
 
 void FlightPilot::poseCallback(const nav_msgs::Odometry::ConstPtr &msg) {
-  state_est_ = quadrotor_common::QuadStateEstimate(*msg);
-
-  quad_state_.p = state_est_.position.cast<Scalar>();
-  quad_state_.qx = state_est_.orientation.coeffs().cast<Scalar>();
+  quad_state_.x[QS::POSX] = (Scalar)msg->pose.pose.position.x;
+  quad_state_.x[QS::POSY] = (Scalar)msg->pose.pose.position.y;
+  quad_state_.x[QS::POSZ] = (Scalar)msg->pose.pose.position.z;
+  quad_state_.x[QS::ATTW] = (Scalar)msg->pose.pose.orientation.w;
+  quad_state_.x[QS::ATTX] = (Scalar)msg->pose.pose.orientation.x;
+  quad_state_.x[QS::ATTY] = (Scalar)msg->pose.pose.orientation.y;
+  quad_state_.x[QS::ATTZ] = (Scalar)msg->pose.pose.orientation.z;
+  //
   quad_ptr_->setState(quad_state_);
 
   if (unity_render_ && unity_ready_) {