adds dynamic reconfigure for odometry calibration of atrvjr node (rflex)

.gitignore

@@ -3,6 +3,8 @@ msg_gen/
 srv_gen/
 **/src/*/msg/
 **/src/*/srv/
+**/**/**/cpp/
+**/**/src/rflex/
 build/
 doc/
 bin/
@@ -19,6 +21,7 @@ docs/
 .project
 .cproject
 .pydevproject
+.settings
 
 # Python
 *.pyc

atrvjr_drivers/rflex/CMakeLists.txt

@@ -11,6 +11,10 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
 rosbuild_init()
 
+rosbuild_find_ros_package(dynamic_reconfigure)
+include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+gencfg()
+
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 

atrvjr_drivers/rflex/cfg/AtrvjrParams.cfg

@@ -0,0 +1,14 @@
+#! /usr/bin/env python
+
+PACKAGE='rflex'
+import roslib
+roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator import *
+
+gen = ParameterGenerator()
+#        Name                   Type       Level    Description                                                           Default  Min   Max
+gen.add("odo_distance_conversion",   int_t,    0,    "Arbitrary units per meter.", 93810,     1, 188000)
+gen.add("odo_angle_conversion",   int_t,    0,    "Arbitrary units per radian.", 38500,     1, 78000)
+
+exit(gen.generate(PACKAGE, "rflex", "AtrvjrParams"))

atrvjr_drivers/rflex/include/rflex/atrvjr_config.h

@@ -27,14 +27,6 @@
 #ifndef ATRVJR_CONFIG_H
 #define ATRVJR_CONFIG_H
 
-// Odometery Constants
-// ===================
-// Arbitrary units per meter
-//#define ODO_DISTANCE_CONVERSION 90810
-#define ODO_DISTANCE_CONVERSION 93810
-// Arbitrary units per radian
-#define ODO_ANGLE_CONVERSION 38500
-
 // Sonar Constants
 // ===============
 // Arbitrary units per meter (for sonar)

atrvjr_drivers/rflex/include/rflex/atrvjr_driver.h

@@ -76,6 +76,9 @@ class ATRVJR : public RFLEX {
             return odomReady==3;
         }
 
+        void setOdoAngleConversion(int);
+        void setOdoDistanceConversion(int);
+
 //        SimpleSignal bumpsUpdateSignal;
     private:
         /**\param ringi BODY_INDEX or BASE_INDEX
@@ -94,6 +97,10 @@ class ATRVJR : public RFLEX {
         int first_distance;
         bool found_distance;
         int home_bearing; ///< Last home bearing (arbitrary units)
+
+        // Odometery Settings
+        int odo_distance_conversion_; ///< Arbitrary units per meter
+        int odo_angle_conversion_; ///< Arbitrary units per radian
 //        int** bumps;
 
         // Not allowed to use these

atrvjr_drivers/rflex/manifest.xml

@@ -13,6 +13,7 @@
   <depend package="nav_msgs" />
   <depend package="tf" />
   <depend package="angles" />
+  <depend package="dynamic_reconfigure" />
 
 </package>
 

atrvjr_drivers/rflex/src/atrvjr_driver.cc

@@ -29,7 +29,10 @@
 #include <math.h>
 #include <cstdio>
 
-ATRVJR::ATRVJR() {
+ATRVJR::ATRVJR():
+    odo_distance_conversion_(93810),
+    odo_angle_conversion_(38500)
+{
     found_distance = false;
 //    bumps = new int*[2];
 //
@@ -54,19 +57,19 @@ float ATRVJR::getDistance() {
         found_distance = true;
     }
 
-    return (distance-first_distance) / (float) ODO_DISTANCE_CONVERSION;
+    return (distance-first_distance) / (float) odo_distance_conversion_;
 }
 
 float ATRVJR::getBearing() {
-    return (bearing-HOME_BEARING) / (float) ODO_ANGLE_CONVERSION;
+    return (bearing-HOME_BEARING) / (float) odo_angle_conversion_;
 }
 
 float ATRVJR::getTranslationalVelocity() const {
-    return transVelocity / (float) ODO_DISTANCE_CONVERSION;
+    return transVelocity / (float) odo_distance_conversion_;
 }
 
 float ATRVJR::getRotationalVelocity() const {
-    return rotVelocity / (float) ODO_ANGLE_CONVERSION;
+    return rotVelocity / (float) odo_angle_conversion_;
 }
 
 float ATRVJR::getVoltage() const {
@@ -116,9 +119,9 @@ void ATRVJR::setSonarPower(bool on) {
 
 void ATRVJR::setMovement( float tvel, float rvel,
                        float acceleration ) {
-    setVelocity(tvel * ODO_DISTANCE_CONVERSION,
-                rvel * ODO_ANGLE_CONVERSION,
-                acceleration * ODO_DISTANCE_CONVERSION);
+    setVelocity(tvel * odo_distance_conversion_,
+                rvel * odo_angle_conversion_,
+                acceleration * odo_distance_conversion_);
 }
 
 
@@ -196,7 +199,7 @@ void ATRVJR::processDioEvent(unsigned char address, unsigned short data) {
 
     if (address == HEADING_HOME_ADDRESS) {
         home_bearing = bearing;
-        printf("ATRVJR Home %f \n", home_bearing / (float) ODO_ANGLE_CONVERSION);
+        printf("ATRVJR Home %f \n", home_bearing / (float) odo_angle_conversion_);
 //    }// check if the dio packet came from a bumper packet
 //    else if ((address >= BUMPER_ADDRESS) && (address < (BUMPER_ADDRESS+BUMPER_COUNT))) {
 //        int index =0, rot = address - BUMPER_ADDRESS;
@@ -212,3 +215,11 @@ void ATRVJR::processDioEvent(unsigned char address, unsigned short data) {
 }
 
 
+void ATRVJR::setOdoAngleConversion(int odo_angle_conversion) {
+    odo_angle_conversion_ = odo_angle_conversion;
+}
+
+
+void ATRVJR::setOdoDistanceConversion(int odo_distance_conversion) {
+    odo_distance_conversion_ = odo_distance_conversion;
+}

atrvjr_drivers/rflex/src/atrvjr_node.cc

@@ -12,6 +12,10 @@
 
 #include <boost/bind.hpp>
 
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include <rflex/AtrvjrParamsConfig.h>
+
 /**
  *  \brief ATRV-JR Node for ROS
  *  By Mikhail Medvedev 02/2012
@@ -64,6 +68,10 @@ class ATRVJRNode {
         void RFLEXSonarUpdateCb();
 //        void RFLEXBumpsUpdateCb();
 
+        // Dynamic reconfigure
+        dynamic_reconfigure::Server<rflex::AtrvjrParamsConfig> reconfigure_srv_;
+        void reconfigureCb(rflex::AtrvjrParamsConfig& config, uint32_t level);
+
     public:
         ros::NodeHandle n;
         ATRVJRNode();
@@ -79,6 +87,17 @@ class ATRVJRNode {
 };
 
 ATRVJRNode::ATRVJRNode() : n ("~") {
+
+    int param;
+    n.param("odo_distance_conversion",param, 93810);
+    driver.setOdoDistanceConversion(param);
+    n.param("odo_angle_conversion",param, 38500);
+    driver.setOdoAngleConversion(param);
+
+    // Setup dynamic reconfigure
+    reconfigure_srv_.setCallback(boost::bind(&ATRVJRNode::reconfigureCb, this, _1, _2));
+
+
     isSonarOn = false;
     initialized = false;
     prev_bumps = 0;
@@ -103,6 +122,12 @@ ATRVJRNode::ATRVJRNode() : n ("~") {
 //    driver.bumpsUpdateSignal.set(boost::bind(&ATRVJRNode::RFLEXBumpsUpdateCb, this));
 }
 
+void ATRVJRNode::reconfigureCb(rflex::AtrvjrParamsConfig& config,
+        uint32_t level) {
+    driver.setOdoDistanceConversion(config.odo_distance_conversion);
+    driver.setOdoAngleConversion(config.odo_angle_conversion);
+}
+
 void ATRVJRNode::RFLEXSystemStatusUpdateCb(){
     std_msgs::Bool bmsg;
     bmsg.data = isSonarOn;