diff --git a/CHANGELOG.md b/CHANGELOG.md index 6383a9ff5c435c22cbf4ed30370f96a2b5cbb3a2..7c7ee583a96780e1ca56a30a7067a22b45a97e01 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ Requires `libfranka` >= 0.7.0 * **BREAKING** Moved `panda_moveit_config` to [`ros-planning`](https://github.com/ros-planning/panda_moveit_config). * Added support for ROS Melodic Morenia. * Raised minimum CMake version to 3.4 to match `libfranka`. + * Add rosparam to choose value of `franka::RealtimeConfig`. ## 0.6.0 - 2018-08-08 diff --git a/Jenkinsfile b/Jenkinsfile index faa665771c71e3cb7d2d13b4588441e5612716a8..e0cb3185a357fe02bf0958444c099908dcfef8a6 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -35,7 +35,7 @@ def getStages(rosDistribution, ubuntuVersion) { } docker.build("franka_ros-ci-worker:${rosDistribution}", - "-f src/franka_ros/.ci/Dockerfile.${rosDistribution} src/franka_ros/.ci").inside { + "-f src/franka_ros/.ci/Dockerfile.${rosDistribution} src/franka_ros/.ci").inside('-e MAKEFLAGS') { withEnv(["CMAKE_PREFIX_PATH+=${env.WORKSPACE}/dist/libfranka/lib/cmake/Franka", "ROS_HOME=${env.WORKSPACE}/ros-home"]) { stage("${rosDistribution}: Build & Lint (Debug)") { diff --git a/franka_control/config/franka_combined_control_node.yaml b/franka_control/config/franka_combined_control_node.yaml index 8fd271c34c7ba1dd1b617ee7c017c606530b38fb..3793c2c767ff3bcff3ea5ce5c5f86a59d63502b7 100644 --- a/franka_control/config/franka_combined_control_node.yaml +++ b/franka_control/config/franka_combined_control_node.yaml @@ -21,6 +21,8 @@ panda_1: cutoff_frequency: 100 # Internal controller for motion generators [joint_impedance|cartesian_impedance] internal_controller: joint_impedance + # Used to decide whether to enforce realtime mode [enforce|ignore] + realtime_config: enforce # Configure the initial defaults for the collision behavior reflexes. collision_config: lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] @@ -51,6 +53,8 @@ panda_2: cutoff_frequency: 100 # Internal controller for motion generators [joint_impedance|cartesian_impedance] internal_controller: joint_impedance + # Used to decide whether to enforce realtime mode [enforce|ignore] + realtime_config: enforce # Configure the initial defaults for the collision behavior reflexes. collision_config: lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml index 5a0c78c807802643ad90b38ca25c2c6e731dc335..b27d5706d75d7b869de8c1a3d91622e88c0a56f2 100644 --- a/franka_control/config/franka_control_node.yaml +++ b/franka_control/config/franka_control_node.yaml @@ -15,6 +15,8 @@ rate_limiting: true cutoff_frequency: 100 # Internal controller for motion generators [joint_impedance|cartesian_impedance] internal_controller: joint_impedance +# Used to decide whether to enforce realtime mode [enforce|ignore] +realtime_config: enforce # Configure the initial defaults for the collision behavior reflexes. collision_config: lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] # [Nm] diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp index fae2f0f6c2c316c8e258dec4b8c46051951c8c18..07dc7107927c045e3f97fcf6a69ca149f790c309 100644 --- a/franka_control/src/franka_control_node.cpp +++ b/franka_control/src/franka_control_node.cpp @@ -31,6 +31,17 @@ int main(int argc, char** argv) { franka::Robot& robot = franka_control.robot(); + std::string realtime_config_param = node_handle.param("realtime_config", std::string("enforce")); + franka::RealtimeConfig realtime_config; + if (realtime_config_param == "enforce") { + realtime_config = franka::RealtimeConfig::kEnforce; + } else if (realtime_config_param == "ignore") { + realtime_config = franka::RealtimeConfig::kIgnore; + } else { + ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'."); + return 1; + } + std::atomic_bool has_error(false); ServiceContainer services; diff --git a/franka_hw/include/franka_hw/franka_hw.h b/franka_hw/include/franka_hw/franka_hw.h index 0140d0228012869a9e8002f009bb8c42e10da53a..8f3730feffa7af0a337188909ee8525cce460b9e 100644 --- a/franka_hw/include/franka_hw/franka_hw.h +++ b/franka_hw/include/franka_hw/franka_hw.h @@ -9,6 +9,7 @@ #include <list> #include <string> +#include <franka/control_types.h> #include <franka/duration.h> #include <franka/model.h> #include <franka/rate_limiting.h> @@ -500,6 +501,7 @@ class FrankaHW : public hardware_interface::RobotHW { std::string robot_ip_; urdf::Model urdf_model_; double joint_limit_warning_threshold_{0.1}; + franka::RealtimeConfig realtime_config_; bool initialized_{false}; std::atomic_bool controller_active_{false}; diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp index c4cce12ab6c5bb9a7815e5d982152b39bfe8495e..40d9ac61e46c7dd9241e90ecfb984cca6e43fcb0 100644 --- a/franka_hw/src/franka_hw.cpp +++ b/franka_hw/src/franka_hw.cpp @@ -127,6 +127,17 @@ bool FrankaHW::initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h joint_limit_warning_threshold_); } + std::string realtime_config_param = robot_hw_nh.param("realtime_config", std::string("enforce")); + franka::RealtimeConfig realtime_config; + if (realtime_config_param == "enforce") { + realtime_config_ = franka::RealtimeConfig::kEnforce; + } else if (realtime_config_param == "ignore") { + realtime_config_ = franka::RealtimeConfig::kIgnore; + } else { + ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'."); + return false; + } + // Get full collision behavior config from the parameter server. std::vector<double> thresholds = getCollisionThresholds("lower_torque_thresholds_acceleration", robot_hw_nh,