Position To Velocity

The position_to_velocity node takes a pose message and converts it to velocity in the NED frame, body level frame, and the camera frame. The transform from the body frame to the camera frame is given as a parameter in a YAML file. The node also publishes the associated covariance. The body level frame, also called the local level frame, is the body frame which is unrolled and unpitched. The figure below will help understand the frames better:

The node currently subscribes only to a geometry_msgs/PoseStamped message type but the code is modular to be able to subscribe to other messages types. To do so, just copy the values into an Eigen::Affine and pass it to the process_msg(...) function.

Table of Contents

1. Prerequisites
2. Installation
3. Node Topics
4. Parameters
5. Usage
6. Comments on Code

Prerequisites

The following packages are requred depencencies:

geometry_msgs
roscpp
rospy
std_msgs
tf2_eigen
reef_msgs

Installation

cd catkin_ws/src
git clone https://github.com/uf-reef-avl/position_to_velocity/
cd ../ && catkin_make

Node Topics:

The node subscribes to the following messages:

/pose_stamped [geometry_msgs/PoseStamped]

The node publishes the following messages:

velocity/ned [geometry_msgs/TwistWithCovarianceStamped]
velocity/camera_frame [geometry_msgs/TwistWithCovarianceStamped]
velocity/body_level_frame [geometry_msgs/TwistWithCovarianceStamped]

Parameters

The node depends on the following parameters:

Parameter	Function	Default	Type
body_to_camera	The transform from body frame to camera frame	None	Set of double, converted to Eigen::Affine3d
convert_to_ned	Flag to convert from NWU to NED frame	False	bool
verbose	Flag to print debug messages	False	bool
alpha	Parameter for the alpha-beta filter	1.0	Double
mocap_noise_std	Standard Deviation for the noise	0.01	Double
update_rate	Update rate in HZ	20	Double

Usage

The node can be launched using the following lines in a launch file

    <node name="pose_to_vel" pkg="position_to_velocity" type="position_to_velocity_node" output="screen" >
        <rosparam command="load" file="$(find position_to_velocity)/params/basic.yaml" />
    </node>

It can also be used via a rosrun command.

Comments on Code

The code uses the std library to generate random numbers.

In line 90 you notice that we get the yaw from the current pose from the rotation matrix portion of the homogeneous transformation (*linear() return a Matrix3d which is the rotation matrix). The function get_yaw expects a Direction Cosine Matrix which is the inverse of the rotation matrix. Since rotation matrices and DCM are orthonormal matrices, the inverse is the transpose.

reef_msgs::get_yaw(current_pose.linear().transpose(), yaw);

Line 101 uses an alpha-beta filter to filter out any noise.

filtered_velocity_NED.translation() = alpha * velocity_current.translation() - (1-alpha) * velocity_previous.translation();