From 6970f5bcbbae4ee0e7fbca787b85c2d8a10d5970 Mon Sep 17 00:00:00 2001
From: Alexandre Vannobel <41082816+alexvannobel@users.noreply.github.com>
Date: Tue, 17 Dec 2019 16:12:36 -0500
Subject: [PATCH] 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
---
.../non-generated/kortex_arm_driver.h | 2 ++
...e_computed_joint_trajectory_action_server.h | 1 +
kortex_driver/launch/kortex_driver.launch | 4 ++++
kortex_driver/readme.md | 2 ++
.../src/non-generated/kortex_arm_driver.cpp | 18 ++++++++++++++++--
kortex_driver/src/non-generated/main.cpp | 2 +-
...computed_joint_trajectory_action_server.cpp | 18 ++++++++++++++----
.../launch/trajectory_execution.launch.xml | 2 +-
.../launch/trajectory_execution.launch.xml | 2 +-
9 files changed, 42 insertions(+), 9 deletions(-)
diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h
index 6b402736..d6992f54 100644
--- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h
+++ b/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;
diff --git a/kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h b/kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h
index 577b023c..82a7a988 100644
--- a/kortex_driver/include/kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h
+++ b/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;
diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch
index fbe12c88..53a0cc1a 100644
--- a/kortex_driver/launch/kortex_driver.launch
+++ b/kortex_driver/launch/kortex_driver.launch
@@ -8,6 +8,8 @@
+
+
@@ -34,6 +36,8 @@
+
+
diff --git a/kortex_driver/readme.md b/kortex_driver/readme.md
index 9db6d1bb..4dd5a9bd 100644
--- a/kortex_driver/readme.md
+++ b/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.
diff --git a/kortex_driver/src/non-generated/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/kortex_arm_driver.cpp
index 7b97089c..dc0fba8b 100644
--- a/kortex_driver/src/non-generated/kortex_arm_driver.cpp
+++ b/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);
diff --git a/kortex_driver/src/non-generated/main.cpp b/kortex_driver/src/non-generated/main.cpp
index 82dde097..ed1c2f82 100644
--- a/kortex_driver/src/non-generated/main.cpp
+++ b/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();
// }
diff --git a/kortex_driver/src/non-generated/pre_computed_joint_trajectory_action_server.cpp b/kortex_driver/src/non-generated/pre_computed_joint_trajectory_action_server.cpp
index c6b056ab..c87bd326 100644
--- a/kortex_driver/src/non-generated/pre_computed_joint_trajectory_action_server.cpp
+++ b/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 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());
diff --git a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml
index c286c5fd..95b4df70 100644
--- a/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml
+++ b/kortex_move_it_config/gen3_move_it_config/launch/trajectory_execution.launch.xml
@@ -9,7 +9,7 @@
-
+
diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml
index e2c24c84..9e035d1d 100644
--- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml
+++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/trajectory_execution.launch.xml
@@ -9,7 +9,7 @@
-
+