Merge pull request #101 from nakane11/frame-count

Add frame_count arg to kxr_controller.launch and set default value as 50

ros/kxr_controller/CMakeLists.txt

@@ -140,6 +140,6 @@ if(CATKIN_ENABLE_TESTING)
   endif()
   add_rostest(test/kxr_controller.test ARGS namespace:="")
   add_rostest(test/kxr_controller.test ARGS namespace:="robot_a")
-  add_rostest(test/kxr_controller.test ARGS namespace:="" use_rcb4:=true)
-  add_rostest(test/kxr_controller.test ARGS namespace:="robot_a" use_rcb4:=true)
+  add_rostest(test/kxr_controller.test ARGS namespace:="" use_rcb4:=true frame_count:=5)
+  add_rostest(test/kxr_controller.test ARGS namespace:="robot_a" use_rcb4:=true frame_count:=5)
 endif()

ros/kxr_controller/cfg/KXRParameteres.cfg

@@ -8,7 +8,7 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator
 
 gen = ParameterGenerator()
 # The value 5 for frame_count was experimentally confirmed to enable smooth interpolation
-gen.add("frame_count", int_t, 0, "Frame Count", 5, 1, 255)
+gen.add("frame_count", int_t, 0, "Frame Count", 50, 1, 255)
 gen.add("wheel_frame_count", int_t, 0, "Frame Count for Wheel", 1, 1, 255)
 
 exit(gen.generate(PACKAGE, PACKAGE, "KXRParameteres"))

ros/kxr_controller/launch/kxr_controller.launch

@@ -13,6 +13,7 @@
   <arg name="device" default="" doc="Device path"/>
   <arg name="model_server_port" default="8123" />
   <arg name="joint_group_description" default="" />
+  <arg name="frame_count" default="50" />
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -41,6 +42,7 @@
         control_pressure: $(arg control_pressure)
         imu_frame_id: $(arg namespace)/$(arg imu_frame_id)
         use_rcb4: $(arg use_rcb4)
+        frame_count: $(arg frame_count)
       </rosparam>
     </node>
     <node name="urdf_model_server"
@@ -75,6 +77,7 @@
         control_pressure: $(arg control_pressure)
         imu_frame_id: $(arg imu_frame_id)
         use_rcb4: $(arg use_rcb4)
+        frame_count: $(arg frame_count)
       </rosparam>
     </node>
 

ros/kxr_controller/launch/kxr_controller_for_wheel.launch

@@ -10,6 +10,7 @@
   <arg name="model_server_port" default="8123" />
   <arg name="namespace" default="" />
   <arg name="joint_group_description" default="" />
+  <arg name="frame_count" default="50" />
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch" >
     <arg name="urdf_path" value="$(arg urdf_path)" />
@@ -22,6 +23,7 @@
     <arg name="publish_battery_voltage" value="$(arg publish_battery_voltage)" />
     <arg name="namespace" value="$(arg namespace)" />
     <arg name="joint_group_description" value="$(arg joint_group_description)" />
+    <arg name="frame_count" value="$(arg frame_count)" />
   </include>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >

ros/kxr_controller/test/kxr_controller.test

@@ -2,12 +2,14 @@
 
   <arg name="namespace" default="" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
+  <arg name="frame_count" default="50" />
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch">
     <arg name="urdf_path" value="$(find kxr_models)/urdf/kxr_test_head.urdf" />
     <arg name="servo_config_path" value="$(find kxr_models)/config/kxr_test_head_servo_config.yaml" />
     <arg name="namespace" value="$(arg namespace)" />
     <arg name="use_rcb4" value="$(arg use_rcb4)" />
+    <arg name="frame_count" value="$(arg frame_count)" />
   </include>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >

ros/kxr_controller/test/test_kxr_controller.py

@@ -28,11 +28,11 @@ def setUp(self):
 
     def test_follow_joint_trajectory(self):
         self.ri.servo_on()
-        self.ri.angle_vector(self.robot_model.init_pose(), 1.0)
+        self.ri.angle_vector(self.robot_model.init_pose(), 3.0)
         self.ri.wait_interpolation()
         self.robot_model.HEAD_JOINT0.joint_angle(np.deg2rad(45))
         self.robot_model.HEAD_JOINT1.joint_angle(np.deg2rad(45))
-        self.ri.angle_vector(self.robot_model.angle_vector(), 1.0)
+        self.ri.angle_vector(self.robot_model.angle_vector(), 3.0)
         self.ri.wait_interpolation()
         rospy.sleep(1.0)
         current_angles = self.ri.angle_vector()
@@ -43,7 +43,7 @@ def test_follow_joint_trajectory(self):
             + f"Current angles = {current_angles}, "
             + f"Expected angles = {expected_angles}",
         )
-        self.ri.angle_vector(self.robot_model.init_pose(), 1.0)
+        self.ri.angle_vector(self.robot_model.init_pose(), 3.0)
         self.ri.wait_interpolation()
         self.ri.servo_off()