Various MoveIt improvements (#64)

* Add parameters in the launch file for session username and password

* Various fixes to address problems with short trajectories and notification mismatches in the action server

kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h

@@ -75,6 +75,8 @@ class KortexArmDriver
 
     // Api options
     std::string m_ip_address;
+    std::string m_username;
+    std::string m_password;
     int m_cyclic_data_publish_rate;
     int m_api_rpc_timeout_ms;
     int m_api_session_inactivity_timeout_ms;

kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h

@@ -72,6 +72,7 @@ class PreComputedJointTrajectoryActionServer
         std::chrono::system_clock::time_point m_trajectory_end_time;
 
         std::mutex m_server_state_lock;
+        std::mutex m_action_notification_thread_lock;
         ActionServerState m_server_state;
 
         KortexMathUtil m_math_util;

kortex_driver/launch/kortex_driver.launch

@@ -8,6 +8,8 @@
 
     <!-- Kortex API options -->
     <arg name="ip_address" default="192.168.1.10"/>
+    <arg name="username" default="admin"/>
+    <arg name="password" default="admin"/>
     <arg name="cyclic_data_publish_rate" default="100"/> <!--Hz-->
     <arg name="api_rpc_timeout_ms" default="2000"/> <!--milliseconds-->
     <arg name="api_session_inactivity_timeout_ms" default="35000"/> <!--milliseconds-->
@@ -34,6 +36,8 @@
         <!-- Start the kortex_driver node -->
         <node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
             <param name="ip_address" value="$(arg ip_address)"/>
+            <param name="username" value="$(arg username)"/>
+            <param name="password" value="$(arg password)"/>
             <param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
             <param name="api_rpc_timeout_ms" value="$(arg api_rpc_timeout_ms)"/>
             <param name="api_session_inactivity_timeout_ms" value="$(arg api_session_inactivity_timeout_ms)"/>

kortex_driver/readme.md

@@ -52,6 +52,8 @@ The `kortex_driver` node is the node responsible for the communication between t
 - **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""** and the only other supported option is **robotiq_2f_85** for now.
 - **robot_name** : This is the namespace in which the driver will run. It defaults to **my_$(arg arm)** (so "my_gen3" for arm="gen3").
 - **ip_address** : The IP address of the robot you're connecting to. The default value is **192.168.1.10**.
+- **username** : The username for the robot connection. The default value is **admin**.
+- **password** : The password for the robot connection. The default value is **admin**.
 - **cyclic_data_publish_rate** : Publish rate of the *base_feedback* and *joint_state* topics, in Hz. The default value is **100** Hz.
 - **api_rpc_timeout_ms** : The default X-axis position of the robot in Gazebo. The default value is **0.0**.
 - **api_session_inactivity_timeout_ms** : The duration after which the robot will clean the client session if the client hangs up the connection brutally (should not happen with the ROS driver). The default value is **35000** ms and is not normally changed.

kortex_driver/src/non-generated/kortex_arm_driver.cpp

@@ -97,6 +97,20 @@ void KortexArmDriver::parseRosArguments()
         throw new std::runtime_error(error_string);
     }
 
+    if (!ros::param::get("~username", m_username))
+    {
+        std::string error_string = "Username for the robot session was not specified in the launch file, shutting down the node...";
+        ROS_ERROR("%s", error_string.c_str());
+        throw new std::runtime_error(error_string);
+    }
+
+    if (!ros::param::get("~password", m_password))
+    {
+        std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node...";
+        ROS_ERROR("%s", error_string.c_str());
+        throw new std::runtime_error(error_string);
+    }
+
     if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate))
     {
         std::string error_string = "Publish rate of the cyclic data was not specified in the launch file, shutting down the node...";
@@ -194,8 +208,8 @@ void KortexArmDriver::initApi()
 
     // Create the sessions so we can start using the robot
     auto createSessionInfo = Kinova::Api::Session::CreateSessionInfo();
-	createSessionInfo.set_username("admin");
-	createSessionInfo.set_password("admin");
+	createSessionInfo.set_username(m_username);
+	createSessionInfo.set_password(m_password);
 	createSessionInfo.set_session_inactivity_timeout(m_api_session_inactivity_timeout_ms);
     createSessionInfo.set_connection_inactivity_timeout(m_api_connection_inactivity_timeout_ms);
 

kortex_driver/src/non-generated/main.cpp

@@ -16,7 +16,7 @@ int main(int argc, char **argv)
 {
     ros::init(argc, argv, "kortex_arm_driver");
 
-    // if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
+    // if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
     //     ros::console::notifyLoggerLevelsChanged();
     // }
 

kortex_driver/src/non-generated/pre_computed_joint_trajectory_action_server.cpp

@@ -31,7 +31,7 @@ PreComputedJointTrajectoryActionServer::PreComputedJointTrajectoryActionServer(c
     if (!ros::param::get("~default_goal_tolerance", m_default_goal_tolerance))
     {
         ROS_WARN("Parameter default_goal_tolerance was not specified; assuming 0.005 radians as default value.");
-        m_default_goal_time_tolerance = 0.005;
+        m_default_goal_tolerance = 0.005;
     }
     if (!ros::param::get("~joint_names", m_joint_names))
     {
@@ -154,10 +154,11 @@ void PreComputedJointTrajectoryActionServer::preempt_received_callback(actionlib
 // Called in a separate thread when a notification comes in
 void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api::Base::ActionNotification notif)
 {
-    ROS_DEBUG("Action notification received.");
     Kinova::Api::Base::ActionEvent event = notif.action_event();
     Kinova::Api::Base::ActionHandle handle = notif.handle();
     Kinova::Api::Base::ActionType type = handle.action_type();
+    ROS_DEBUG("Action notification received of type %s", Kinova::Api::Base::ActionEvent_Name(event).c_str());
+    std::lock_guard<std::mutex> guard(m_action_notification_thread_lock);
 
     control_msgs::FollowJointTrajectoryResult result;
     std::ostringstream oss;
@@ -190,6 +191,14 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api::
                 ROS_INFO("Preprocessing has finished in the arm and goal has been accepted.");
                 set_server_state(ActionServerState::TRAJECTORY_EXECUTION_PENDING);
             }
+            // FIXME KOR-3563 Sometimes the notifications arrive in the wrong order so it is possible to receive 
+            // a ACTION_PREPROCESS_END notification after the ACTION_START
+            // When this bug will be fixed this else if can be removed
+            else if (m_server_state == ActionServerState::TRAJECTORY_EXECUTION_IN_PROGRESS)
+            {
+                ROS_DEBUG("Notification order mismatch : We received the ACTION_PREPROCESS_END after the ACTION_START");
+                break;
+            }
             // We should not have received that
             else
             {
@@ -244,7 +253,8 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api::
         // The arm is starting to move
         case Kinova::Api::Base::ActionEvent::ACTION_START:
             // The preprocessing was done and the goal is still active (not preempted)
-            if ((m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING) &&
+            if ((m_server_state == ActionServerState::TRAJECTORY_EXECUTION_PENDING ||
+                 m_server_state == ActionServerState::PRE_PROCESSING_IN_PROGRESS) && // FIXME KOR-3563 this happens if we received a ACTION_START before a ACTION_PREPROCESS_END
                  m_goal.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
             {
                 ROS_INFO("Trajectory has started.");
@@ -326,7 +336,7 @@ void PreComputedJointTrajectoryActionServer::action_notif_callback(Kinova::Api::
                 else
                 {
                     result.error_code = result.PATH_TOLERANCE_VIOLATED;
-                    oss << "After validation, trajectory execution failed in the arm with sub error code " << notif.abort_details();
+                    oss << "After validation, trajectory execution failed in the arm with sub error code " << Kinova::Api::SubErrorCodes_Name(notif.abort_details());
                     result.error_string = oss.str();
 
                     ROS_ERROR("%s", oss.str().c_str());

kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml

@@ -9,7 +9,7 @@
   <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
   <param name="trajectory_execution/allowed_execution_duration_scaling" value="2.0"/> <!-- default 1.2 -->
   <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
-  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="2.0"/> <!-- default is 0.5 but this needs to be higher before preprocessing in the arm takes time -->
   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
   <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
 

kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml

@@ -9,7 +9,7 @@
   <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
   <param name="trajectory_execution/allowed_execution_duration_scaling" value="2.0"/> <!-- default 1.2 -->
   <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
-  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="2.0"/> <!-- default is 0.5 but this needs to be higher before preprocessing in the arm takes time -->
   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
   <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->