Interacting with the on-board state estimator


#1

HI!

We’re trying to stabilize our drone using optic flow from a downward facing camera. We’re performing all the displacement calculations on-board as a ros node on the flytpod. Which ros topic should we make use of for passing all the estimated displacements onto the state estimator?

Thank you!


#2

Hi,

Look out for vision_pose_estimate or vision_speed_estimate in mavros_extras.


#3

I’m using the vision_position_estimate mavlink message type now. I’ll keep you posted!


#4

So I’m publishing the odometry data on mavros/vision_pose/pose topic.

What’s the best way to visualize this data?

I tried using copter_visualization node from mavros_extras but that only subscribes to mavros/local_position/local

I can try to publish the data on this topic, but it expects the data to be in geometry_msgs/PoseStamped format while the flytpod gives it in geometry_msgs/TwistStamped format


#5

In rviz?

Please dont publish on this topic, as it would cause CRITICAL issues in working of FlytOS.

Take a look at its code, and create you own version of it.

I have this code snippet for you in case it helps, but mind you this won’t work of the shelf:

lposCb(const geometry_msgs::TwistStampedConstPtr &lpos)

{
lpos_ptr = lpos;
if (!track_marker)
{
track_marker = boost::make_shared<visualization_msgs::Marker>();
track_marker->type = visualization_msgs::Marker::CUBE_LIST;
track_marker->ns = “trajectory”;
track_marker->action = visualization_msgs::Marker::ADD;
track_marker->scale.x = marker_scale * 0.010;
track_marker->scale.y = marker_scale * 0.010;
track_marker->scale.z = marker_scale * 0.010;
track_marker->color.a = 1.0;
track_marker->color.r = 1.0;
track_marker->color.g = 0.0;
track_marker->color.b = 0.0;
track_marker->points.reserve(max_track_size);
}

geometry_msgs::Point pnt;
pnt.x = lpos_ptr->twist.linear.y;
pnt.y = lpos_ptr->twist.linear.x;
pnt.z = -lpos_ptr->twist.linear.z;
if ( track_marker->points.size() < max_track_size )
  track_marker->points.push_back(pnt);
else 
	track_marker->points[marker_idx] = pnt;
marker_idx = ++marker_idx % max_track_size;
track_marker->header = lpos_ptr->header;
track_marker->header.frame_id = "iris/camera_rgbd/second_camera_depth_optical_frame";
_track_marker_pub.publish(track_marker);

}


#6

Thank you, I’m able to visualize the trajectory now!


#7

Hi,
Could you throw more light on how the estimator works i.e. how it picks up the flow position/velocity estimates?

And on which topic the local position data is being published by the estimator?