I am trying to send local odometry position to the vehicle_visual_odometry subscriber. I am using ros2.
First I convert the position I get from gazebo simulation (ENU) to NED using the following code
/* start publishing to px4*/
/* transform positions*/
px4_ros_com::frame_transforms::StaticTF transform = px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED;
Eigen::Vector3d pos_ned, vel_ned,ang_vel_ned;
Eigen::Vector3d pos_enu(this->current_sim_pose.position.x,this->current_sim_pose.position.y,this->current_sim_pose.position.z);
Eigen::Vector3d vel_enu(this->current_sim_twist.linear.x,this->current_sim_twist.linear.y,this->current_sim_twist.linear.z);
Eigen::Vector3d ang_vel_enu(this->current_sim_twist.angular.x,this->current_sim_twist.angular.y,this->current_sim_twist.angular.z);
pos_ned = transform_static_frame(pos_enu, transform);
vel_ned = transform_static_frame(vel_enu, transform);
ang_vel_ned = transform_static_frame(ang_vel_enu, transform);
/* transform orientation*/
Eigen::Quaterniond quat_ned_orien;
Eigen::Quaterniond quat_enu_orien(this->current_sim_pose.orientation.w,this->current_sim_pose.orientation.x,this->current_sim_pose.orientation.y,this->current_sim_pose.orientation.z);
// double roll, pitch, yaw;
px4_ros_com::frame_transforms::StaticTF transform = px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED;
Eigen::Vector3d pos_ned, vel_ned,ang_vel_ned;
Eigen::Vector3d pos_enu(this->current_sim_pose.position.x,this->current_sim_pose.position.y,this->current_sim_pose.position.z);
Eigen::Vector3d vel_enu(this->current_sim_twist.linear.x,this->current_sim_twist.linear.y,this->current_sim_twist.linear.z);
Eigen::Vector3d ang_vel_enu(this->current_sim_twist.angular.x,this->current_sim_twist.angular.y,this->current_sim_twist.angular.z);
pos_ned = transform_static_frame(pos_enu, transform);
vel_ned = transform_static_frame(vel_enu, transform);
ang_vel_ned = transform_static_frame(ang_vel_enu, transform);
/* transform orientation*/
Eigen::Quaterniond quat_ned_orien;
Eigen::Quaterniond quat_enu_orien(this->current_sim_pose.orientation.w,this->current_sim_pose.orientation.x,this->current_sim_pose.orientation.y,this->current_sim_pose.orientation.z);
// double roll, pitch, yaw;
// px4_ros_com::frame_transforms::utils::quaternion::quaternion_to_euler(quat_enu_orien, roll, pitch, yaw);
// std::cout << "roll: " << roll << " Pitch: " << pitch << " yaw: " << yaw <<std::endl;
px4_ros_com::frame_transforms::StaticTF transform = px4_ros_com::frame_transforms::StaticTF::ENU_TO_NED;
quat_ned_orien = transform_orientation(quat_enu_orien, transform);
/* send data to px4*/
this->current_odom_to_px4.timestamp = this->timestamp;
this->current_odom_to_px4.timestamp_sample = this->timestamp_sample;
/* positions*/
this->current_odom_to_px4.local_frame = 0; // LOCAL_FRAME_NED
this->current_odom_to_px4.x = pos_ned(0);
this->current_odom_to_px4.y = pos_ned(1);
this->current_odom_to_px4.z = pos_ned(2);
this->current_odom_to_px4.q[0] = quat_ned_orien.w();
this->current_odom_to_px4.q[1] = quat_ned_orien.x();
this->current_odom_to_px4.q[2] = quat_ned_orien.y();
this->current_odom_to_px4.q[3] = quat_ned_orien.z();
/* vecloity*/
this->current_odom_to_px4.velocity_frame = 0; // LOCAL_FRAME_NED
this->current_odom_to_px4.vx = vel_ned(0);
this->current_odom_to_px4.vy = vel_ned(1);
this->current_odom_to_px4.vz = vel_ned(2);
this->current_odom_to_px4.rollspeed = ang_vel_ned(0);
this->current_odom_to_px4.pitchspeed = ang_vel_ned(1);
this->current_odom_to_px4.yawspeed = ang_vel_ned(2);
this->vioToPx4_publisher->publish(this->current_odom_to_px4);
Then following the instructions in the vio, I set the following parameters
EKF2_AID_MASK 280, I want the position, velocity and yaw from the odometry I created.
EKF2_HGT_MODE 3
everytime I execute the command takeoff I get
takeoff denied! please rearm and retry
arming denied! manual control lost
I try arming it, but it does not seem to work. Ís there something I am doing
wrong
can you help me please? @priseborough @mhkabir