Merge pull request #40 from byu-magicc/yaml

YAML

CMakeLists.txt

@@ -2,15 +2,16 @@ cmake_minimum_required(VERSION 2.8.3)
 project(fcu_io)
 
 find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
   fcu_common
   message_generation
   roscpp
   sensor_msgs
   std_msgs
-  cmake_modules
 )
-find_package(Eigen REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(Eigen REQUIRED)
+pkg_check_modules(YAML yaml-cpp)
 
 ###################################
 ## catkin specific configuration ##
@@ -24,6 +25,7 @@ find_package(Boost REQUIRED COMPONENTS system thread)
 
 add_service_files(
   FILES
+  ParamFile.srv
   ParamGet.srv
   ParamSet.srv
 )
@@ -37,7 +39,7 @@ catkin_package(
   INCLUDE_DIRS include
   LIBRARIES mavrosflight
   CATKIN_DEPENDS roscpp sensor_msgs std_msgs
-  DEPENDS Boost
+  DEPENDS Boost Eigen yaml-cpp
 )
 
 ###########
@@ -48,6 +50,8 @@ include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  ${YAML_INCLUDEDIR}
 )
 
 # mavrosflight library
@@ -65,6 +69,7 @@ add_dependencies(mavrosflight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPOR
 target_link_libraries(mavrosflight
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${YAML_LIBRARIES}
 )
 
 # fcu_io_node

include/fcu_io.h

@@ -25,6 +25,7 @@
 #include <fcu_common/ExtendedCommand.h>
 #include <fcu_common/ServoOutputRaw.h>
 
+#include <fcu_io/ParamFile.h>
 #include <fcu_io/ParamGet.h>
 #include <fcu_io/ParamSet.h>
 
@@ -74,6 +75,8 @@ class fcuIO :
   bool paramGetSrvCallback(fcu_io::ParamGet::Request &req, fcu_io::ParamGet::Response &res);
   bool paramSetSrvCallback(fcu_io::ParamSet::Request &req, fcu_io::ParamSet::Response &res);
   bool paramWriteSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
+  bool paramSaveToFileCallback(fcu_io::ParamFile::Request &req, fcu_io::ParamFile::Response &res);
+  bool paramLoadFromFileCallback(fcu_io::ParamFile::Request &req, fcu_io::ParamFile::Response &res);
   bool calibrateImuBiasSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
   bool calibrateImuTempSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
   bool calibrateRCTrimSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
@@ -107,6 +110,8 @@ class fcuIO :
   ros::ServiceServer param_get_srv_;
   ros::ServiceServer param_set_srv_;
   ros::ServiceServer param_write_srv_;
+  ros::ServiceServer param_save_to_file_srv_;
+  ros::ServiceServer param_load_from_file_srv_;
   ros::ServiceServer imu_calibrate_bias_srv_;
   ros::ServiceServer imu_calibrate_temp_srv_;
   ros::ServiceServer calibrate_rc_srv_;

include/mavrosflight/param.h

@@ -9,6 +9,8 @@
 #include <mavrosflight/mavlink_bridge.h>
 #include <mavrosflight/mavlink_serial.h>
 
+#include <yaml-cpp/yaml.h>
+
 namespace mavrosflight
 {
 
@@ -22,11 +24,10 @@ class Param
   uint16_t pack_param_set_msg(uint8_t system, uint8_t component, mavlink_message_t *msg,
                               uint8_t target_system, uint8_t target_component);
 
-  std::string getName();
-  int getIndex();
-  MAV_PARAM_TYPE getType();
-  double getValue();
-  bool getSetInProgress();
+  std::string getName() const;
+  int getIndex() const;
+  MAV_PARAM_TYPE getType() const;
+  double getValue() const;
 
   void requestSet(double value, mavlink_message_t *msg);
   bool handleUpdate(const mavlink_param_value_t &msg);

include/mavrosflight/param_manager.h

@@ -36,6 +36,9 @@ class ParamManager : public MavlinkListenerInterface
   void register_param_listener(ParamListenerInterface *listener);
   void unregister_param_listener(ParamListenerInterface *listener);
 
+  bool save_to_file(std::string filename);
+  bool load_from_file(std::string filename);
+
 private:
   void request_param_list();
 

src/fcu_io.cpp

@@ -23,6 +23,8 @@ fcuIO::fcuIO()
   param_get_srv_ = nh_.advertiseService("param_get", &fcuIO::paramGetSrvCallback, this);
   param_set_srv_ = nh_.advertiseService("param_set", &fcuIO::paramSetSrvCallback, this);
   param_write_srv_ = nh_.advertiseService("param_write", &fcuIO::paramWriteSrvCallback, this);
+  param_save_to_file_srv_ = nh_.advertiseService("param_save_to_file", &fcuIO::paramSaveToFileCallback, this);
+  param_load_from_file_srv_ = nh_.advertiseService("param_load_from_file", &fcuIO::paramLoadFromFileCallback, this);
   imu_calibrate_bias_srv_ = nh_.advertiseService("calibrate_imu_bias", &fcuIO::calibrateImuBiasSrvCallback, this);
   imu_calibrate_temp_srv_ = nh_.advertiseService("calibrate_imu_temp", &fcuIO::calibrateImuTempSrvCallback, this);
   calibrate_rc_srv_ = nh_.advertiseService("calibrate_rc_trim", &fcuIO::calibrateRCTrimSrvCallback, this);
@@ -591,6 +593,18 @@ bool fcuIO::paramWriteSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Tri
   return true;
 }
 
+bool fcuIO::paramSaveToFileCallback(ParamFile::Request &req, ParamFile::Response &res)
+{
+  res.success = mavrosflight_->param.save_to_file(req.filename);
+  return true;
+}
+
+bool fcuIO::paramLoadFromFileCallback(ParamFile::Request &req, ParamFile::Response &res)
+{
+  res.success = mavrosflight_->param.load_from_file(req.filename);
+  return true;
+}
+
 bool fcuIO::calibrateImuBiasSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
 {
   mavlink_message_t msg;

src/mavrosflight/param.cpp

@@ -15,7 +15,11 @@ Param::Param()
 
 Param::Param(mavlink_param_value_t msg)
 {
-  init(std::string(msg.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN),
+  char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+  memcpy(name, msg.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+  name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+
+  init(std::string(name),
        msg.param_index,
        (MAV_PARAM_TYPE) msg.param_type,
        msg.param_value);
@@ -26,22 +30,22 @@ Param::Param(std::string name, int index, MAV_PARAM_TYPE type, float raw_value)
   init(name, index, type, raw_value);
 }
 
-std::string Param::getName()
+std::string Param::getName() const
 {
   return name_;
 }
 
-int Param::getIndex()
+int Param::getIndex() const
 {
   return index_;
 }
 
-MAV_PARAM_TYPE Param::getType()
+MAV_PARAM_TYPE Param::getType() const
 {
   return type_;
 }
 
-double Param::getValue()
+double Param::getValue() const
 {
   return value_;
 }

src/mavrosflight/param_manager.cpp

@@ -5,6 +5,10 @@
 
 #include <mavrosflight/param_manager.h>
 
+#include <yaml-cpp/yaml.h>
+
+#include <fstream>
+
 namespace mavrosflight
 {
 
@@ -130,6 +134,69 @@ void ParamManager::unregister_param_listener(ParamListenerInterface *listener)
   }
 }
 
+bool ParamManager::save_to_file(std::string filename)
+{
+  // build YAML document
+  YAML::Emitter yaml;
+  yaml << YAML::BeginSeq;
+  std::map<std::string, Param>::iterator it;
+  for (it = params_.begin(); it != params_.end(); it++)
+  {
+    yaml << YAML::Flow;
+    yaml << YAML::BeginMap;
+    yaml << YAML::Key << "name" << YAML::Value << it->second.getName();
+    yaml << YAML::Key << "type" << YAML::Value << (int) it->second.getType();
+    yaml << YAML::Key << "value" << YAML::Value << it->second.getValue();
+    yaml << YAML::EndMap;
+  }
+  yaml << YAML::EndSeq;
+
+  // write to file
+  try
+  {
+    std::ofstream fout;
+    fout.open(filename.c_str());
+    fout << yaml.c_str();
+    fout.close();
+  }
+  catch (...)
+  {
+    return false;
+  }
+
+  return true;
+}
+
+bool ParamManager::load_from_file(std::string filename)
+{
+  try
+  {
+    YAML::Node root = YAML::LoadFile(filename);
+    assert(root.IsSequence());
+
+    for (int i = 0; i < root.size(); i++)
+    {
+      if (root[i].IsMap() && root[i]["name"] && root[i]["type"] && root[i]["value"])
+      {
+        if (is_param_id(root[i]["name"].as<std::string>()))
+        {
+          Param param = params_.find(root[i]["name"].as<std::string>())->second;
+          if ((MAV_PARAM_TYPE) root[i]["type"].as<int>() == param.getType())
+          {
+            set_param_value(root[i]["name"].as<std::string>(), root[i]["value"].as<double>());
+          }
+        }
+      }
+    }
+
+    return true;
+  }
+  catch (...)
+  {
+    return false;
+  }
+}
+
 void ParamManager::request_param_list()
 {
   mavlink_message_t param_list_msg;

srv/ParamFile.srv

@@ -0,0 +1,3 @@
+string filename
+---
+bool success