diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp index 6b9ffcff3bdc59f43f914bcc0275eaf34bee2f40..2b8d78300e2c6b6146dbf8e38aaf6496270e5508 100644 --- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp +++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp @@ -89,15 +89,15 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms } void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg) { - this->state.p(0) = vel_msg->values[0]; - this->state.p(1) = vel_msg->values[1]; - this->state.p(2) = vel_msg->values[2]; + this->state.p(0) = vel_msg->values[0]/1000.0; + this->state.p(1) = vel_msg->values[1]/1000.0; + this->state.p(2) = vel_msg->values[2]/1000.0; } void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &pose_msg) { - this->state.q(0) = pose_msg->values[0]; - this->state.q(1) = pose_msg->values[1]; - this->state.q(2) = pose_msg->values[2]; + this->state.q(0) = pose_msg->values[0]/1000.0; + this->state.q(1) = pose_msg->values[1]/1000.0; + this->state.q(2) = pose_msg->values[2]/1000.0; this->state.q(3) = pose_msg->values[3]; // degres } @@ -113,7 +113,7 @@ void robotState::stateCallbackUgvAcc(const geometry_msgs::Vector3ConstPtr &acc_m void robotState::stateCallbackUavAcc(const crazyswarm::GenericLogDataConstPtr &Acc_msg) { - this->state.a(0) = Acc_msg->values[0]; - this->state.a(1) = Acc_msg->values[1]; - this->state.a(2) = Acc_msg->values[2]; + this->state.a(0) = Acc_msg->values[0]/1000.0; + this->state.a(1) = Acc_msg->values[1]/1000.0; + this->state.a(2) = Acc_msg->values[2]/1000.0; }