From 29e87a687f16254a726783f835107922b92849ee Mon Sep 17 00:00:00 2001 From: ajshank Date: Sat, 14 Nov 2020 18:15:41 -0600 Subject: [PATCH] deprecate velctrl msgs, remove old refs to common_msgs, redefine as freyja_msgs --- freyja_msgs/CMakeLists.txt | 4 ++-- freyja_msgs/msg/lqr_ctrl/CtrlCommand.msg | 5 +++-- freyja_msgs/msg/lqr_vel_ctrl/ControllerDebug.msg | 10 ---------- freyja_msgs/msg/lqr_vel_ctrl/CtrlCommand.msg | 9 --------- freyja_msgs/msg/state_manager/CurrentState.msg | 1 + freyja_msgs/package.xml | 11 ++++------- state_manager/CMakeLists.txt | 2 +- state_manager/include/state_estimator.h | 14 +++++++------- state_manager/include/state_manager.h | 2 +- state_manager/package.xml | 4 ++-- state_manager/src/callback_implementations.cpp | 10 +++++----- state_manager/src/state_estimator.cpp | 4 ++-- state_manager/src/state_manager.cpp | 2 +- 13 files changed, 29 insertions(+), 49 deletions(-) delete mode 100644 freyja_msgs/msg/lqr_vel_ctrl/ControllerDebug.msg delete mode 100644 freyja_msgs/msg/lqr_vel_ctrl/CtrlCommand.msg diff --git a/freyja_msgs/CMakeLists.txt b/freyja_msgs/CMakeLists.txt index 73297c4..abcf581 100644 --- a/freyja_msgs/CMakeLists.txt +++ b/freyja_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(common_msgs) +project(freyja_msgs) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -53,7 +53,7 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES common_msgs +# LIBRARIES freyja_msgs # CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp std_msgs # DEPENDS system_lib ) diff --git a/freyja_msgs/msg/lqr_ctrl/CtrlCommand.msg b/freyja_msgs/msg/lqr_ctrl/CtrlCommand.msg index 45a94ab..b19894d 100644 --- a/freyja_msgs/msg/lqr_ctrl/CtrlCommand.msg +++ b/freyja_msgs/msg/lqr_ctrl/CtrlCommand.msg @@ -1,8 +1,9 @@ # Control commands for the attitude controller. # Need to translate differently for AscTec HLP/LLP, PX4, AC etc. # Attitude in radians, thrust in *Newtons*. No scaling! -# ctrl_mode is 8 bits (asctec): [ x x gps height thrust yaw roll pitch ] - +# ctrl_mode is: +# 8 bits (asctec): [ x x gps height thrust yaw roll pitch ] +# 8 bits (apm) : [ x x x x x x yawrate ctrlvalid] float64 roll float64 pitch float64 yaw diff --git a/freyja_msgs/msg/lqr_vel_ctrl/ControllerDebug.msg b/freyja_msgs/msg/lqr_vel_ctrl/ControllerDebug.msg deleted file mode 100644 index ee51acc..0000000 --- a/freyja_msgs/msg/lqr_vel_ctrl/ControllerDebug.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Debug message for controller. -# State can be invalid due to time delay, or a dead source. - -Header header -float64[4] lqr_u -float64 thrust -float64 roll -float64 pitch -float64 yaw -uint8 state_valid diff --git a/freyja_msgs/msg/lqr_vel_ctrl/CtrlCommand.msg b/freyja_msgs/msg/lqr_vel_ctrl/CtrlCommand.msg deleted file mode 100644 index b6dcbad..0000000 --- a/freyja_msgs/msg/lqr_vel_ctrl/CtrlCommand.msg +++ /dev/null @@ -1,9 +0,0 @@ -# Control commands for the AscTec HLP/LLP -# Expect rpyt to be in -1 .. +1 double -# ctrl_mode is 8 bits: [ x x gps height thrust yaw roll pitch ] - -float64 roll -float64 pitch -float64 yaw -float64 thrust -uint8 ctrl_mode diff --git a/freyja_msgs/msg/state_manager/CurrentState.msg b/freyja_msgs/msg/state_manager/CurrentState.msg index 042f4bb..34a6427 100644 --- a/freyja_msgs/msg/state_manager/CurrentState.msg +++ b/freyja_msgs/msg/state_manager/CurrentState.msg @@ -2,3 +2,4 @@ # [pn, pe, pd, vn, ve, vd, roll, pitch, yaw, rrate, prate, yrate, delta_t] Header header float64[13] state_vector +uint8 state_valid diff --git a/freyja_msgs/package.xml b/freyja_msgs/package.xml index 9bae2ac..02ac401 100644 --- a/freyja_msgs/package.xml +++ b/freyja_msgs/package.xml @@ -1,16 +1,13 @@ - common_msgs + freyja_msgs 0.0.0 - The common_msgs package + The freyja_msgs package - - - - aj + aj - TODO + GPLv3 catkin diff --git a/state_manager/CMakeLists.txt b/state_manager/CMakeLists.txt index 5ff3ec5..c521507 100644 --- a/state_manager/CMakeLists.txt +++ b/state_manager/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs - common_msgs + freyja_msgs sensor_msgs nav_msgs ) diff --git a/state_manager/include/state_estimator.h b/state_manager/include/state_estimator.h index b2a0c68..3c7945b 100644 --- a/state_manager/include/state_estimator.h +++ b/state_manager/include/state_estimator.h @@ -23,15 +23,15 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "aj_filter_collection.cpp" -typedef common_msgs::ControllerDebug::ConstPtr LqrInputAccel; +typedef freyja_msgs::ControllerDebug::ConstPtr LqrInputAccel; #if LQE_INTEGRATOR_ORDER_ == 2 const int nStates = 9; @@ -80,7 +80,7 @@ class StateEstimator std::chrono::duration prop_interval_; // final ros data published - common_msgs::CurrentStateBiasFree state_msg_; + freyja_msgs::CurrentStateBiasFree state_msg_; public: StateEstimator(); @@ -90,7 +90,7 @@ class StateEstimator // listen to state information from an external source ros::Subscriber state_sub_; - void stateUpdatesCallback( const common_msgs::CurrentState::ConstPtr & ); + void stateUpdatesCallback( const freyja_msgs::CurrentState::ConstPtr & ); // listen to full control commands (atti + accel) from the controller ros::Subscriber ctrl_input_sub_; diff --git a/state_manager/include/state_manager.h b/state_manager/include/state_manager.h index d0bb2ab..fd860be 100644 --- a/state_manager/include/state_manager.h +++ b/state_manager/include/state_manager.h @@ -86,7 +86,7 @@ class StateManager /* Callback handler for asctec_onboard_data */ ros::Subscriber asctec_data_sub_; - void asctecDataCallback( const common_msgs::AsctecData::ConstPtr & ); + void asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr & ); /* Callback handlers for mavros data */ ros::Subscriber mavros_gps_sub_; diff --git a/state_manager/package.xml b/state_manager/package.xml index a7c72fd..e70ce24 100644 --- a/state_manager/package.xml +++ b/state_manager/package.xml @@ -5,14 +5,14 @@ The state_manager package aj - Risky + GPLv3 catkin message_generation roscpp std_msgs geometry_msgs - common_msgs + freyja_msgs sensor_msgs nav_msgs diff --git a/state_manager/src/callback_implementations.cpp b/state_manager/src/callback_implementations.cpp index d6369ad..99f6043 100644 --- a/state_manager/src/callback_implementations.cpp +++ b/state_manager/src/callback_implementations.cpp @@ -74,14 +74,14 @@ void StateManager::viconCallback( const TFStamped::ConstPtr &msg ) last_yaw_ = yaw; /* Copy over and publish right away */ - common_msgs::CurrentState state_msg; + 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::asctecDataCallback( const common_msgs::AsctecData::ConstPtr &msg ) +void StateManager::asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr &msg ) { double time_since = (ros::Time::now() - lastUpdateTime_).toSec(); double x, y, z, vx, vy, vz; @@ -145,7 +145,7 @@ void StateManager::asctecDataCallback( const common_msgs::AsctecData::ConstPtr & last_pd_ = state_vector_[2]; /* Copy over and publish right away */ - common_msgs::CurrentState state_msg; + 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]; @@ -185,7 +185,7 @@ void StateManager::mavrosGpsVelCallback( const TwStamped::ConstPtr &msg ) lastUpdateTime_ = ros::Time::now(); - common_msgs::CurrentState state_msg; + 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]; @@ -253,7 +253,7 @@ void StateManager::cameraUpdatesCallback( const CameraOdom::ConstPtr &msg ) state_vector_[8] = yaw; // publish away! - common_msgs::CurrentState state_msg; + 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]; diff --git a/state_manager/src/state_estimator.cpp b/state_manager/src/state_estimator.cpp index 8cdb1e5..6fb6302 100644 --- a/state_manager/src/state_estimator.cpp +++ b/state_manager/src/state_estimator.cpp @@ -22,7 +22,7 @@ StateEstimator::StateEstimator() : nh_(), priv_nh_("~") std::string output_topic; priv_nh_.param( "output_topic", output_topic, std::string("/current_state_estimate") ); // for when all is said and done - state_pub_ = nh_.advertise + state_pub_ = nh_.advertise ( output_topic, 1, true ); int estimator_rate_default = 70; @@ -261,7 +261,7 @@ void StateEstimator::medianFilter() */ } -void StateEstimator::stateUpdatesCallback( const common_msgs::CurrentState::ConstPtr & msg ) +void StateEstimator::stateUpdatesCallback( const freyja_msgs::CurrentState::ConstPtr & msg ) { // no need to mutexify this, since measurement_z_ is only used in update function diff --git a/state_manager/src/state_manager.cpp b/state_manager/src/state_manager.cpp index 9281f0f..d1ca367 100644 --- a/state_manager/src/state_manager.cpp +++ b/state_manager/src/state_manager.cpp @@ -29,7 +29,7 @@ StateManager::StateManager() : nh_(), priv_nh_("~") /* Announce state publisher */ - state_pub_ = nh_.advertise + state_pub_ = nh_.advertise ( "current_state", 1, true ); /* Instantiate filters. Useful for vicon. Autopilots have their own filters */