Merge pull request #321 from YoheiKakiuchi/fix_r2.0.0

configure controller settings from param
seed-solutions · Apr 6, 2018 · ec9eb5a · ec9eb5a
2 parents b2e5f74 + ac13eea
commit ec9eb5a
Show file tree

Hide file tree

Showing 4 changed files with 61 additions and 8 deletions.
diff --git a/aero_description/typeF/robots/controller_settings.yaml b/aero_description/typeF/robots/controller_settings.yaml
@@ -43,3 +43,5 @@ rhand_controller:
   type: position_controllers/JointTrajectoryController
   joints:
     - r_thumb_joint
+
+robot_interface_controllers: ['larm', 'rarm', 'head', 'waist', 'lifter']
diff --git a/aero_ros_controller/include/aero_ros_controller/RobotInterface.hh b/aero_ros_controller/include/aero_ros_controller/RobotInterface.hh
@@ -240,6 +240,7 @@ public:
   bool updateJointList();
   bool updateJointList(std::vector < std::string > &_controller_names_list);
 
+  bool configureFromParam(const std::string &_param);
 private:
   //// callback
   void JointStateCallback_(const sensor_msgs::JointState::ConstPtr& _msg);

diff --git a/aero_ros_controller/src/RobotInterface.cc b/aero_ros_controller/src/RobotInterface.cc
@@ -477,15 +477,20 @@ bool RobotInterface::add_controller (const std::string &_key,
                                      const std::vector< std::string> &_jnames,
                                      bool _update_joint_list)
 {
-  boost::mutex::scoped_lock lock(states_mtx_);
-  boost::shared_ptr<TrajectoryClient > p(new TrajectoryClient(local_nh_, _action_name, _state_name, _jnames));
-  if (!p->isServerConnected()) {
-    return false;
-  }
-  if(_update_joint_list) {
-    std::copy( _jnames.begin(), _jnames.end(), std::back_inserter(joint_list_) );
+  boost::shared_ptr<TrajectoryClient > p;
+  {
+    boost::mutex::scoped_lock lock(states_mtx_);
+    p.reset(new TrajectoryClient(local_nh_, _action_name, _state_name, _jnames));
+
+    if (!p->isServerConnected()) {
+      return false;
+    }
+
+    if(_update_joint_list) {
+      std::copy( _jnames.begin(), _jnames.end(), std::back_inserter(joint_list_) );
+    }
+    p->setName(_key);
   }
-  p->setName(_key);
   return this->add_controller(_key, p);
 }
 
@@ -554,6 +559,34 @@ bool RobotInterface::wait_interpolation_(const std::string &_name, double _tm)
   return ret;
 }
 
+bool RobotInterface::configureFromParam(const std::string &_param)
+{
+  if (local_nh_.hasParam(_param)) {
+    std::vector<std::string > lst;
+    local_nh_.getParam(_param, lst);
+    for(int i = 0; i < lst.size(); i++) {
+      ROS_DEBUG("controller: %s", lst[i].c_str());
+      std::string p = lst[i] + "_controller/joints";
+      if(local_nh_.hasParam(p)) {
+        std::vector<std::string > jt;
+        local_nh_.getParam(p, jt);
+        for(int j = 0; j < jt.size(); j++) {
+          ROS_DEBUG("  j_%d: %s", j, jt[j].c_str());
+        }
+        if (jt.size() > 0) {
+          add_controller(lst[i],
+                         lst[i] + "_controller/follow_joint_trajectory",
+                         lst[i] + "_controller/state",
+                         jt);
+        }
+      }
+    }
+  } else {
+    ROS_WARN("there is no param: %s%s",
+             local_nh_.getNamespace().c_str(), _param.c_str());
+  }
+}
+
 //// callback
 void RobotInterface::JointStateCallback_(const sensor_msgs::JointState::ConstPtr& _msg)
 {

diff --git a/aero_std/include/aero_std/AeroRobotInterface.hh b/aero_std/include/aero_std/AeroRobotInterface.hh
@@ -11,6 +11,7 @@ public:
 
 public:
   AeroRobotInterface(ros::NodeHandle &_nh) : robot_interface::RobotInterface(_nh) {
+#if 0
     // rarm
     rarm.reset(new robot_interface::TrajectoryClient(_nh,
                                     "rarm_controller/follow_joint_trajectory",
@@ -56,6 +57,22 @@ public:
                                     { "neck_y_joint", "neck_p_joint", "neck_r_joint"}
                                     ));
     this->add_controller("head",   head);
+#endif
+    //
+    configureFromParam("robot_interface_controllers");
+#define ADD_CONTROLLER(limb)                    \
+    {                                           \
+      auto it = controllers_.find(#limb);       \
+      if (it != controllers_.end()) {           \
+        this-> limb = it->second;               \
+      }                                         \
+    }
+
+    ADD_CONTROLLER(larm);
+    ADD_CONTROLLER(rarm);
+    ADD_CONTROLLER(head);
+    ADD_CONTROLLER(waist);
+    ADD_CONTROLLER(lifter);
 
     // group settings
     controller_group_["both_arms"]  = {"rarm", "larm"};