Merge pull request #228 from miguelprada/velocity_iface_tests

Add tests for velocity_controllers::JointTrajectoryController

Author: bmagyar

joint_trajectory_controller/CMakeLists.txt

@@ -88,6 +88,11 @@ if(CATKIN_ENABLE_TESTING)
                     test/joint_trajectory_controller.test
                     test/joint_trajectory_controller_test.cpp)
   target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES})
+
+  add_rostest_gtest(joint_trajectory_controller_vel_test
+                    test/joint_trajectory_controller_vel.test
+                    test/joint_trajectory_controller_test.cpp)
+  target_link_libraries(joint_trajectory_controller_vel_test ${catkin_LIBRARIES})
 endif()
 
 # Install

joint_trajectory_controller/test/joint_trajectory_controller_vel.test

@@ -0,0 +1,35 @@
+<launch>
+  <!-- Load RRbot model -->
+  <param name="robot_description"
+      command="$(find xacro)/xacro.py '$(find joint_trajectory_controller)/test/rrbot.xacro'" />
+
+  <!-- Start RRbot -->
+  <node name="rrbot"
+      pkg="joint_trajectory_controller"
+      type="rrbot"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find joint_trajectory_controller)/test/rrbot_vel_controllers.yaml" />
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner" output="screen"
+        args="rrbot_controller" />
+
+  <!-- Rxplot monitoring -->
+  <node name="rrbot_pos_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/rrbot_controller/state/desired/positions[0]:positions[1],/rrbot_controller/state/actual/positions[0]:positions[1]" />
+
+  <node name="rrbot_vel_monitor"
+        pkg="rqt_plot"
+        type="rqt_plot"
+        args="/rrbot_controller/state/desired/velocities[0]:velocities[1],/rrbot_controller/state/actual/velocities[0]:velocities[1]" />
+
+  <!-- Controller test -->
+  <test test-name="joint_trajectory_controller_vel_test"
+        pkg="joint_trajectory_controller"
+        type="joint_trajectory_controller_test"
+        time-limit="80.0"/>
+</launch>

joint_trajectory_controller/test/rrbot.cpp

@@ -47,7 +47,8 @@ class RRbot : public hardware_interface::RobotHW
     pos_[0] = 0.0; pos_[1] = 0.0;
     vel_[0] = 0.0; vel_[1] = 0.0;
     eff_[0] = 0.0; eff_[1] = 0.0;
-    cmd_[0] = 0.0; cmd_[1] = 0.0;
+    pos_cmd_[0] = 0.0; pos_cmd_[1] = 0.0;
+    vel_cmd_[0] = 0.0; vel_cmd_[1] = 0.0;
 
     // Connect and register the joint state interface
     hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]);
@@ -59,14 +60,23 @@ class RRbot : public hardware_interface::RobotHW
     registerInterface(&jnt_state_interface_);
 
     // Connect and register the joint position interface
-    hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &cmd_[0]);
+    hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &pos_cmd_[0]);
     jnt_pos_interface_.registerHandle(pos_handle_1);
 
-    hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &cmd_[1]);
+    hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &pos_cmd_[1]);
     jnt_pos_interface_.registerHandle(pos_handle_2);
 
     registerInterface(&jnt_pos_interface_);
 
+    // Connect and register the joint velocity interface
+    hardware_interface::JointHandle vel_handle_1(jnt_state_interface_.getHandle("joint1"), &vel_cmd_[0]);
+    jnt_vel_interface_.registerHandle(vel_handle_1);
+
+    hardware_interface::JointHandle vel_handle_2(jnt_state_interface_.getHandle("joint2"), &vel_cmd_[1]);
+    jnt_vel_interface_.registerHandle(vel_handle_2);
+
+    registerInterface(&jnt_vel_interface_);
+
     // Smoothing subscriber
     smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this);
     smoothing_.initRT(0.0);
@@ -82,21 +92,67 @@ class RRbot : public hardware_interface::RobotHW
     const double smoothing = *(smoothing_.readFromRT());
     for (unsigned int i = 0; i < 2; ++i)
     {
-      vel_[i] = (cmd_[i] - pos_[i]) / getPeriod().toSec();
+      if(active_interface_[i] == "hardware_interface::PositionJointInterface")
+      {
+        vel_[i] = (pos_cmd_[i] - pos_[i]) / getPeriod().toSec();
+
+        const double next_pos = smoothing * pos_[i] +  (1.0 - smoothing) * pos_cmd_[i];
+        pos_[i] = next_pos;        
+      }
+      else if(active_interface_[i] == "hardware_interface::VelocityJointInterface")
+      {
+        vel_[i] = (1.0 - smoothing) * vel_cmd_[i];
+        pos_[i] = pos_[i] + vel_[i] * getPeriod().toSec();
+      }
+    }
+  }
+
+  typedef std::list<hardware_interface::ControllerInfo> ControllerList;
 
-      const double next_pos = smoothing * pos_[i] +  (1.0 - smoothing) * cmd_[i];
-      pos_[i] = next_pos;
+  bool prepareSwitch(const ControllerList& start_list,
+                     const ControllerList& stop_list)
+  {
+    for(ControllerList::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
+    {
+      for(std::vector<hardware_interface::InterfaceResources>::const_iterator iface_it = it->claimed_resources.begin(); iface_it != it->claimed_resources.end(); ++iface_it)
+      {
+        for(std::set<std::string>::const_iterator res_it = iface_it->resources.begin(); res_it != iface_it->resources.end(); ++res_it)
+        {
+          if(*res_it == "joint1")
+          {
+            next_active_interface_[0] = iface_it->hardware_interface;
+          }
+          else if(*res_it == "joint2")
+          {
+            next_active_interface_[1] = iface_it->hardware_interface;
+          }
+        }
+      }
     }
+
+    return true;
+  }
+
+  void doSwitch(const ControllerList& start_list,
+                const ControllerList& stop_list)
+  {
+    active_interface_[0] = next_active_interface_[0];
+    active_interface_[1] = next_active_interface_[1];
   }
 
 private:
   hardware_interface::JointStateInterface    jnt_state_interface_;
   hardware_interface::PositionJointInterface jnt_pos_interface_;
-  double cmd_[2];
+  hardware_interface::VelocityJointInterface jnt_vel_interface_;
+  double pos_cmd_[2];
+  double vel_cmd_[2];
   double pos_[2];
   double vel_[2];
   double eff_[2];
 
+  std::string active_interface_[2];
+  std::string next_active_interface_[2];
+
   realtime_tools::RealtimeBuffer<double> smoothing_;
   void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);}
 

joint_trajectory_controller/test/rrbot_vel_controllers.yaml

@@ -0,0 +1,18 @@
+rrbot_controller:
+  type: "velocity_controllers/JointTrajectoryController"
+  joints:
+    - joint1
+    - joint2
+
+  constraints:
+    goal_time: 0.5
+    joint1:
+      goal:       0.01
+      trajectory: 0.05
+    joint2:
+      goal:       0.01
+      trajectory: 0.05
+
+  gains:
+    joint1: {p: 60.0,  i: 0.0, d: 0.0, i_clamp: 1}
+    joint2: {p: 60.0,  i: 0.0, d: 0.0, i_clamp: 1}