Fix jog arm CI integration test (moveit#1466)

* rename .test launch file and re-enable jog-arm integration test
* fix array out-of-bounds access
  drop hard-coded plain c array and directly add Eigen vectors without loop
* start from non-singular robot state

moveit_experimental/jog_arm/CMakeLists.txt

@@ -30,5 +30,5 @@ install(
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   find_package(ros_pytest REQUIRED)
-  add_rostest(test/launch/jog_arm_integration_test.launch)
+  add_rostest(test/launch/jog_arm_integration_test.test)
 endif()

moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp

@@ -505,22 +505,14 @@ double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_veloc
   // direction. Start with a scaled version of the singular vector
   Eigen::VectorXd delta_x(6);
   double scale = 100;
-  delta_x[0] = vector_toward_singularity[0] / scale;
-  delta_x[1] = vector_toward_singularity[1] / scale;
-  delta_x[2] = vector_toward_singularity[2] / scale;
-  delta_x[3] = vector_toward_singularity[3] / scale;
-  delta_x[4] = vector_toward_singularity[4] / scale;
-  delta_x[5] = vector_toward_singularity[5] / scale;
+  delta_x = vector_toward_singularity / scale;
 
   // Calculate a small change in joints
-  Eigen::VectorXd new_delta_theta = pseudo_inverse_ * delta_x;
+  Eigen::VectorXd new_theta;
+  kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta);
+  new_theta += pseudo_inverse_ * delta_x;
+  kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta);
 
-  double theta[6];
-  const double* prev_joints = kinematic_state_->getVariablePositions();
-  for (std::size_t i = 0, size = static_cast<std::size_t>(num_joints_); i < size; ++i)
-    theta[i] = prev_joints[i] + new_delta_theta(i);
-
-  kinematic_state_->setVariablePositions(theta);
   jacobian_ = kinematic_state_->getJacobian(joint_model_group_);
   Eigen::JacobiSVD<Eigen::MatrixXd> new_svd = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian_);
   double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1);

moveit_experimental/jog_arm/test/arm_setup/config/initial_position.yaml

@@ -0,0 +1,10 @@
+zeros:
+  panda_joint1: 0
+  panda_joint2: -0.785
+  panda_joint3: 0
+  panda_joint4: -2.356
+  panda_joint5: 0
+  panda_joint6: 1.571
+  panda_joint7: 0.785
+publish_default_positions: True
+

moveit_experimental/jog_arm/test/integration/test_jog_arm_integration.py

@@ -74,6 +74,6 @@ def test_jog_arm_generates_joint_trajectory_when_joint_jog_command_is_received(n
     received = []
     rospy.sleep(test_duration)
     # test_duration/publish_period is the expected number of messages in this duration.
-    # Allow a small +/- window of 3 due to small rounding/timing errors
-    assert len(received) > test_duration/publish_period - 3
-    assert len(received) < test_duration/publish_period + 3
+     # Allow a small +/- window due to rounding/timing errors
+    assert len(received) >= test_duration/publish_period - 5
+    assert len(received) <= test_duration/publish_period + 5

moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.launch → moveit_experimental/jog_arm/test/launch/jog_arm_integration_test.test

@@ -18,7 +18,9 @@
     <param name="publish_frequency" type="double" value="50.0"/>
   </node>
 
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+	  <rosparam command="load" file="$(find moveit_experimental)/jog_arm/test/arm_setup/config/initial_position.yaml" />
+  </node>
 
   <group ns="jog_arm_server" if="$(arg debug)">
     <rosparam command="load" file="$(find moveit_experimental)/jog_arm/test/arm_setup/config/jog_settings.yaml"/>