fix: ensure changes work with new upstream

This commit ensures the `set_franka_model_configuration` works with the changes made in 51c4c7e. It also simplifies the `SetJointConfiguration.srv` message.
frankaemika · Jan 3, 2024 · 6c8206c · 6c8206c
1 parent 61d9210
commit 6c8206c
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 21 deletions.
diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
@@ -416,7 +416,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
       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()) {
+        if (request.joint_names.size() == 1 && request.joint_names[0].empty()) {
           ROS_ERROR_STREAM_NAMED("franka_hw_sim",
                                  "Failed to set Franka model configuration: the request is "
                                  "invalid because no joints were specified.");
@@ -426,7 +426,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
         }
 
         // Check if 'position' and 'name' fields have different lengths
-        if (request.configuration.name.size() != request.configuration.position.size()) {
+        if (request.joint_names.size() != request.joint_positions.size()) {
           ROS_ERROR_STREAM_NAMED(
               "franka_hw_sim",
               "Failed to set Franka model configuration: the request is invalid because the "
@@ -438,9 +438,9 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
 
         // 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] << ", ";
+        for (std::size_t ii = 0; ii < request.joint_names.size(); ii++) {
+          requested_configuration_stream << request.joint_names[ii] << ": "
+                                         << request.joint_positions[ii] << ", ";
         }
         std::string requested_configuration_string = requested_configuration_stream.str();
         requested_configuration_string = requested_configuration_string.substr(
@@ -450,9 +450,9 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
 
         // Validate joint names and joint limits
         std::unordered_map<std::string, double> 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]);
+        for (std::size_t ii = 0; ii < request.joint_names.size(); ii++) {
+          std::string joint(request.joint_names[ii]);
+          double position(request.joint_positions[ii]);
 
           auto joint_iterator = this->joints_.find(joint);
           if (joint_iterator != this->joints_.end()) {  // If joint exists
@@ -487,18 +487,6 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
           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";
@@ -520,6 +508,7 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) {
                               "Setting of Franka model configuration failed since "
                               "a problem occurred when setting the joint configuration in Gazebo.");
         response.success = false;
+        response.error = "gazebo service call failed";
         return false;
       });
   this->service_controller_list_ = nh.serviceClient<controller_manager_msgs::ListControllers>(

diff --git 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<std::mutex> 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
@@ -1,4 +1,5 @@
-sensor_msgs/JointState configuration
+string[] joint_names
+float64[] joint_positions
 ---
 bool success
 string error