Skip to content

Commit

Permalink
add gps_odom support on state_manager
Browse files Browse the repository at this point in the history 
*major restructure of mavros gps callbacks
*deprecate raw gps lat/lon, keep for sanity checks
*add map (un-)locking for arming and rtk offsets
  • Loading branch information
@ajshank
ajshank committed Dec 30, 2020
1 parent ed6092d commit 9f17061
Show file tree
Hide file tree
Showing 5 changed files with 135 additions and 28 deletions.
1 change: 1 addition & 0 deletions state_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
freyja_msgs
sensor_msgs
nav_msgs
std_srvs
)


Expand Down
50 changes: 43 additions & 7 deletions state_manager/include/state_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,12 @@
#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
#include <tf/tf.h>
#include <std_srvs/SetBool.h>

#include <freyja_msgs/CurrentState.h>
#include <freyja_msgs/AsctecData.h>
Expand All @@ -24,6 +27,8 @@
typedef geometry_msgs::TransformStamped TFStamped;
typedef geometry_msgs::TwistStamped TwStamped;
typedef nav_msgs::Odometry CameraOdom;
typedef std_srvs::SetBool::Request BoolServReq;
typedef std_srvs::SetBool::Response BoolServRsp;

#define DEG2RAD(D) ((D)*3.1415326/180.0)
#define F_PI 3.14153
Expand Down Expand Up @@ -69,13 +74,21 @@ class StateManager
FreyjaFilters pose_filter_;
FreyjaFilters rate_filter_;

/* containers for handling gps data */
double home_lat_, home_lon_;
bool have_location_fix_;
double compass_yaw_;

bool have_arming_origin_;
double map_rtk_pn_, map_rtk_pe_, map_rtk_pd_;
double arming_gps_pn_, arming_gps_pe_, arming_gps_pd_;
double gps_odom_pn_, gps_odom_pe_, gps_odom_pd_;
double rtk_baseoffset_pn_, rtk_baseoffset_pe_, rtk_baseoffset_pd_;

public:
StateManager();
/* launch-time parameter specifies which one to pick */
void initApmManager();
void initPixhawkManager();
void initAsctecManager();
void initViconManager();
void initCameraManager();
Expand All @@ -87,17 +100,40 @@ class StateManager
/* Callback handler for asctec_onboard_data */
ros::Subscriber asctec_data_sub_;
void asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr & );

/* Callback handlers for mavros data */
ros::Subscriber mavros_gps_sub_;
ros::Subscriber mavros_vel_sub_;
void mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr & );
void mavrosGpsVelCallback( const TwStamped::ConstPtr & );

/* Callback handler for camera updates */
ros::Subscriber camera_estimate_sub_;
void cameraUpdatesCallback( const CameraOdom::ConstPtr & );

/* Callback handlers for mavros data */
ros::Subscriber mavros_gpsraw_sub_;
ros::Subscriber mavros_vel_sub_;
ros::Subscriber compass_sub_;
ros::Subscriber mavros_gpsodom_sub_;
ros::Subscriber mavros_rtk_sub_;
void mavrosGpsOdomCallback( const nav_msgs::Odometry::ConstPtr & );
void mavrosCompassCallback( const std_msgs::Float64::ConstPtr & );
void mavrosRtkBaselineCallback( const geometry_msgs::Vector3::ConstPtr & );

void mavrosGpsRawCallback( const sensor_msgs::NavSatFix::ConstPtr & );

/* handlers for locking map frame origins */
inline void lockArmingGps( bool _lock = true )
{
arming_gps_pn_ = _lock? gps_odom_pn_ : 0.0;
arming_gps_pe_ = _lock? gps_odom_pe_ : 0.0;
arming_gps_pd_ = _lock? gps_odom_pd_ : 0.0;
}
inline void lockMapRTK( bool _lock = true )
{
map_rtk_pn_ = _lock? rtk_baseoffset_pn_ : 0.0;
map_rtk_pe_ = _lock? rtk_baseoffset_pe_ : 0.0;
map_rtk_pd_ = _lock? rtk_baseoffset_pd_ : 0.0;
}

ros::ServiceServer maplock_srv_;
bool maplockArmingHandler( BoolServReq&, BoolServRsp& );


/* Publisher for state information */
ros::Publisher state_pub_;
Expand Down
2 changes: 2 additions & 0 deletions state_manager/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<build_depend>freyja_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_srvs</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
Expand All @@ -23,5 +24,6 @@
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_srvs</run_depend>

</package>
89 changes: 74 additions & 15 deletions state_manager/src/callback_implementations.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ void StateManager::viconCallback( const TFStamped::ConstPtr &msg )
last_yaw_ = yaw;

/* Copy over and publish right away */
freyja_msgs::CurrentState state_msg;
static freyja_msgs::CurrentState state_msg;
state_msg.header.stamp = ros::Time::now();
for( uint8_t idx = 0; idx < STATE_VECTOR_LEN; idx++ )
state_msg.state_vector[idx] = state_vector_[idx];
Expand Down Expand Up @@ -152,11 +152,12 @@ void StateManager::asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr &
state_pub_.publish( state_msg );
}

void StateManager::mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr& msg )
void StateManager::mavrosGpsRawCallback( const sensor_msgs::NavSatFix::ConstPtr& msg )
{
/*
if( msg -> status.status >= 0 && !have_location_fix_ )
{
/* first location fix */
// first location fix
home_lat_ = msg -> latitude;
home_lon_ = msg -> longitude;
have_location_fix_ = true;
Expand All @@ -167,30 +168,88 @@ void StateManager::mavrosGpsCallback( const sensor_msgs::NavSatFix::ConstPtr& ms
state_vector_[0] = ( (msg->latitude)/10000000.0 - home_lat_ )*111050.51;
state_vector_[1] = ( (msg->longitude)/10000000.0 - home_lon_ )*84356.28;
*/
}



void StateManager::mavrosCompassCallback( const std_msgs::Float64::ConstPtr &msg )
{
compass_yaw_ = msg -> data;
}

void StateManager::mavrosGpsVelCallback( const TwStamped::ConstPtr &msg )
void StateManager::mavrosGpsOdomCallback( const nav_msgs::Odometry::ConstPtr &msg )
{
if( !have_location_fix_ )
/* Callback for odometry direct from the Pixhawk.
Note that this is Pixhawk's best estimate of where it is in the world,
there is no need to filter it, only structure it to our format.
Likely topics:
/mavros/global_position/local --> does not zero at arming.
/mavros/local_position/local --> zeros at arming, has IMU frame quirk.
*/

static double pn, pe, pd, vn, ve, vd;

// update containers anyway (needed for capturing arming location)
gps_odom_pn_ = msg -> pose.pose.position.y;
gps_odom_pe_ = msg -> pose.pose.position.x;
gps_odom_pd_ = -( msg -> pose.pose.position.z );

if( !have_arming_origin_ )
{
ROS_INFO_THROTTLE( 5, "StateManager:: Waiting for arming .." );
return;
}

double time_since = (ros::Time::now() - lastUpdateTime_).toSec();
// armed at this point
pn = gps_odom_pn_ + map_rtk_pn_ - arming_gps_pn_;
pe = gps_odom_pe_ + map_rtk_pe_ - arming_gps_pe_;
pd = gps_odom_pd_ + map_rtk_pd_ - arming_gps_pd_;

state_vector_[3] = msg -> twist.linear.x;
state_vector_[4] = msg -> twist.linear.y;
state_vector_[5] = msg -> twist.linear.z;
vn = msg -> twist.twist.linear.y;
ve = msg -> twist.twist.linear.x;
vd = -( msg -> twist.twist.linear.z );

state_vector_[12] = time_since;
static freyja_msgs::CurrentState state_msg;
state_msg.state_vector[0] = pn;
state_msg.state_vector[1] = pe;
state_msg.state_vector[2] = pd;
state_msg.state_vector[3] = vn;
state_msg.state_vector[4] = ve;
state_msg.state_vector[5] = vd;
state_msg.state_vector[8] = DEG2RAD( compass_yaw_ );

lastUpdateTime_ = ros::Time::now();

freyja_msgs::CurrentState state_msg;
state_msg.header.stamp = ros::Time::now();
for( uint8_t idx = 0; idx < STATE_VECTOR_LEN; idx++ )
state_msg.state_vector[idx] = state_vector_[idx];
state_pub_.publish( state_msg );
}


void StateManager::mavrosRtkBaselineCallback( const geometry_msgs::Vector3::ConstPtr &msg )
{
rtk_baseoffset_pn_ = msg -> x;
rtk_baseoffset_pe_ = msg -> y;
rtk_baseoffset_pd_ = msg -> z;
}

bool StateManager::maplockArmingHandler( BoolServReq& rq, BoolServRsp& rp )
{
/* Service handler for when the vehicle is armed or disarmed.
This must lock in/release all the map offsets needed by manager.
*/
if( !have_arming_origin_ )
{
lockArmingGps(); // set this locatin as origin
lockMapRTK(); // capture current offset from rtk base (zero if no rtk)
have_arming_origin_ = true;
ROS_WARN( "StateManager::Origin set, locking RTK map-frame!" );
}
else
{
lockArmingGps( false );
lockMapRTK( false );
have_arming_origin_ = false;
ROS_WARN( "StateManager::Origin cleared, unlocking RTK map-frame!" );
}
}

void StateManager::cameraUpdatesCallback( const CameraOdom::ConstPtr &msg )
Expand Down
21 changes: 15 additions & 6 deletions state_manager/src/state_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ StateManager::StateManager() : nh_(), priv_nh_("~")
else if( state_source_ == "asctec" )
initAsctecManager();
else if (state_source_ == "apm" )
initApmManager();
initPixhawkManager();
else if( state_source_ == "onboard_camera" )
initCameraManager();

Expand Down Expand Up @@ -98,12 +98,21 @@ void StateManager::initAsctecManager()
&StateManager::asctecDataCallback, this );
}

void StateManager::initApmManager()
void StateManager::initPixhawkManager()
{
mavros_gps_sub_ = nh_.subscribe( "/mavros/global_position/global", 1,
&StateManager::mavrosGpsCallback, this );
mavros_vel_sub_ = nh_.subscribe( "/mavros/global_position/gp_vel", 1,
&StateManager::mavrosGpsVelCallback, this );
have_arming_origin_ = false;
mavros_gpsraw_sub_ = nh_.subscribe( "/mavros/global_position/global", 1,
&StateManager::mavrosGpsRawCallback, this );
mavros_gpsodom_sub_ = nh_.subscribe( "/mavros/global_position/local", 1,
&StateManager::mavrosGpsOdomCallback, this,
ros::TransportHints().tcpNoDelay() );
mavros_rtk_sub_ = nh_.subscribe( "/ublox_f9p_rtkbaseline", 1,
&StateManager::mavrosRtkBaselineCallback, this );
compass_sub_ = nh_.subscribe( "/mavros/global_position/compass_hdg", 1,
&StateManager::mavrosCompassCallback, this );

maplock_srv_ = nh_.advertiseService( "/lock_arming_maps",
&StateManager::maplockArmingHandler, this );
}

void StateManager::initCameraManager()
Expand Down

0 comments on commit 9f17061

Please sign in to comment.