From bae0507f070dfac1a2a1b76bb7d5767ebac8cf7a Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Wed, 3 Jan 2024 21:40:56 +0100 Subject: [PATCH] fix: ensure changes work with new upstream This commit ensures the `set_franka_model_configuration` works with the changes made in 89d25713b741631ca9f4f417585e7835d160cf4a. It also simplifies the `SetJointConfiguration.srv` message. --- franka_gazebo/src/franka_hw_sim.cpp | 116 ---------------------- franka_gazebo/src/joint.cpp | 2 + franka_msgs/srv/SetJointConfiguration.srv | 3 +- 3 files changed, 4 insertions(+), 117 deletions(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 2fcc8f72..fb80907b 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -9,10 +9,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -24,8 +22,6 @@ #include #include #include -#include -#include "ros/ros.h" namespace franka_gazebo { @@ -412,122 +408,10 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { response.success = true; return true; }); - this->service_set_model_configuration_ = franka_hw::advertiseService< - franka_msgs::SetJointConfiguration>( - nh, "set_franka_model_configuration", [&](auto& request, auto& response) { - // Check if joints are specified - if (request.configuration.name.size() == 1 && request.configuration.name[0].empty()) { - ROS_ERROR_STREAM_NAMED("franka_hw_sim", - "Failed to set Franka model configuration: the request is " - "invalid because no joints were specified."); - response.success = false; - response.error = "no joints specified"; - return false; - } - - // Check if 'position' and 'name' fields have different lengths - if (request.configuration.name.size() != request.configuration.position.size()) { - ROS_ERROR_STREAM_NAMED( - "franka_hw_sim", - "Failed to set Franka model configuration: the request is invalid because the " - "'position' and 'name' fields have different lengths."); - response.success = false; - response.error = "'position' and 'name' fields length unequal"; - return false; - } - - // Log request information - std::ostringstream requested_configuration_stream; - for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { - requested_configuration_stream << request.configuration.name[ii] << ": " - << request.configuration.position[ii] << ", "; - } - std::string requested_configuration_string = requested_configuration_stream.str(); - requested_configuration_string = requested_configuration_string.substr( - 0, requested_configuration_string.length() - 2); // Remove last 2 characters - ROS_INFO_STREAM_NAMED("franka_hw_sim", - "Setting joint configuration: " << requested_configuration_string); - - // Validate joint names and joint limits - std::unordered_map model_configuration; - for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { - std::string joint(request.configuration.name[ii]); - double position(request.configuration.position[ii]); - - auto joint_iterator = this->joints_.find(joint); - if (joint_iterator != this->joints_.end()) { // If joint exists - double min_position(joint_iterator->second->limits.min_position); - double max_position(joint_iterator->second->limits.max_position); - - // Check if position is within joint limits - if (position == boost::algorithm::clamp(position, min_position, max_position)) { - model_configuration.emplace(std::make_pair(joint, position)); - } else { - ROS_WARN_STREAM_NAMED("franka_hw_sim", - "Joint configuration of joint '" - << joint - << "' was not set since the requested joint position (" - << position << ") is not within joint limits (i.e. " - << min_position << " - " << max_position << ")."); - } - } else { - ROS_WARN_STREAM_NAMED("franka_hw_sim", - "Joint configuration of joint '" - << joint << "' not set since it is not a valid panda joint."); - } - } - - // Return if no valid positions were found - if (model_configuration.empty()) { - ROS_ERROR_STREAM_NAMED("franka_hw_sim", - "Setting of Franka model configuration aborted since no valid " - "joint configuration were found."); - response.success = false; - response.error = "no valid joint configurations"; - return false; - } - - // Throw warnings about unused request fields - if (!request.configuration.effort.empty()) { - ROS_WARN_STREAM_ONCE_NAMED( - "franka_hw_sim", - "The 'set_franka_model_configuration' service does not use the 'effort' field."); - } - if (!request.configuration.velocity.empty()) { - ROS_WARN_STREAM_ONCE_NAMED( - "franka_hw_sim", - "The 'set_franka_model_configuration' service does not use the 'velocity' field."); - } - - // Call Gazebo 'set_model_configuration' service and update Franka joint positions - gazebo_msgs::SetModelConfiguration gazebo_model_configuration; - gazebo_model_configuration.request.model_name = "panda"; - for (const auto& pair : model_configuration) { - gazebo_model_configuration.request.joint_names.push_back(pair.first); - gazebo_model_configuration.request.joint_positions.push_back(pair.second); - } - if (this->service_gazebo_set_model_configuration_.call(gazebo_model_configuration)) { - // Update franka positions - for (const auto& pair : model_configuration) { - this->joints_[pair.first]->setJointPosition(pair.second); - } - - response.success = true; - return true; - } - - ROS_WARN_STREAM_NAMED("franka_hw_sim", - "Setting of Franka model configuration failed since " - "a problem occurred when setting the joint configuration in Gazebo."); - response.success = false; - return false; - }); this->service_controller_list_ = nh.serviceClient( "controller_manager/list_controllers"); this->service_controller_switch_ = nh.serviceClient( "controller_manager/switch_controller"); - this->service_gazebo_set_model_configuration_ = - nh.serviceClient("/gazebo/set_model_configuration"); } void FrankaHWSim::restartControllers() { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index c12b3fe9..919dc329 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -13,6 +13,8 @@ void Joint::update(const ros::Duration& dt) { if (this->setPositionRequested_) { std::lock_guard lock(this->requestedPositionMutex_); this->position = this->requestedPosition_; + this->desired_position = this->requestedPosition_; + this->stop_position = this->requestedPosition_; this->setPositionRequested_ = false; } diff --git a/franka_msgs/srv/SetJointConfiguration.srv b/franka_msgs/srv/SetJointConfiguration.srv index f33b2bb6..b4a3e55d 100644 --- a/franka_msgs/srv/SetJointConfiguration.srv +++ b/franka_msgs/srv/SetJointConfiguration.srv @@ -1,4 +1,5 @@ -sensor_msgs/JointState configuration +string[] joint_names +float64[] joint_positions --- bool success string error