<commit_before>#include <franka_hw/franka_hw.h>

#include <mutex>

#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <realtime_tools/realtime_publisher.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/MultiArrayDimension.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <urdf/model.h>

#include "franka_hw_helper_functions.h"

namespace franka_hw {

constexpr double FrankaHW::kMaximumJointAcceleration;
constexpr double FrankaHW::kMaximumJointJerk;

FrankaHW::FrankaHW(const std::vector<std::string>& joint_names,
                   franka::Robot* robot,
                   double publish_rate,
                   const std::string& arm_id,
                   const ros::NodeHandle& node_handle)
    : joint_state_interface_(),
      franka_joint_state_interface_(),
      franka_cartesian_state_interface_(),
      position_joint_interface_(),
      velocity_joint_interface_(),
      effort_joint_interface_(),
      franka_pose_cartesian_interface_(),
      franka_velocity_cartesian_interface_(),
      position_joint_limit_interface_(),
      velocity_joint_limit_interface_(),
      effort_joint_limit_interface_(),
      publish_rate_(publish_rate),
      publisher_transforms_(node_handle, "/tf", 1),
      publisher_franka_states_(node_handle, "franka_states", 1),
      publisher_joint_states_(node_handle, "joint_states", 1),
      publisher_external_wrench_(node_handle, "F_ext", 1),
      joint_names_(),
      arm_id_(arm_id),
      robot_(robot),
      robot_state_(),
      position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      pose_cartesian_command_({1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
                               1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
      velocity_cartesian_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      default_run_function_([this](std::function<bool()> ros_callback) {
        robot_->read(std::bind(&FrankaHW::readCallback, this, ros_callback,
                               std::placeholders::_1));
      }),
      run_function_(default_run_function_) {
  joint_names_.resize(joint_names.size());
  joint_names_ = joint_names;

  for (size_t i = 0; i < joint_names_.size(); ++i) {
    hardware_interface::JointStateHandle joint_handle(
        joint_names_[i], &robot_state_.q[i], &robot_state_.dq[i],
        &robot_state_.tau_J[i]);
    joint_state_interface_.registerHandle(joint_handle);

    FrankaJointStateHandle franka_joint_handle(
        joint_names_[i], robot_state_.q[i], robot_state_.dq[i],
        robot_state_.tau_J[i], robot_state_.q_d[i], robot_state_.dtau_J[i],
        robot_state_.tau_ext_hat_filtered[i], robot_state_.joint_collision[i],
        robot_state_.joint_contact[i]);
    franka_joint_state_interface_.registerHandle(franka_joint_handle);

    hardware_interface::JointHandle position_joint_handle(
        joint_state_interface_.getHandle(joint_names_[i]),
        &position_joint_command_.q[i]);
    position_joint_interface_.registerHandle(position_joint_handle);

    hardware_interface::JointHandle velocity_joint_handle(
        joint_state_interface_.getHandle(joint_names_[i]),
        &velocity_joint_command_.dq[i]);
    velocity_joint_interface_.registerHandle(velocity_joint_handle);
<commit_msg>franka_hw: Use q_d for JointStateHandle.<commit_after>#include <franka_hw/franka_hw.h>

#include <mutex>

#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <realtime_tools/realtime_publisher.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/MultiArrayDimension.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <urdf/model.h>

#include "franka_hw_helper_functions.h"

namespace franka_hw {

constexpr double FrankaHW::kMaximumJointAcceleration;
constexpr double FrankaHW::kMaximumJointJerk;

FrankaHW::FrankaHW(const std::vector<std::string>& joint_names,
                   franka::Robot* robot,
                   double publish_rate,
                   const std::string& arm_id,
                   const ros::NodeHandle& node_handle)
    : joint_state_interface_(),
      franka_joint_state_interface_(),
      franka_cartesian_state_interface_(),
      position_joint_interface_(),
      velocity_joint_interface_(),
      effort_joint_interface_(),
      franka_pose_cartesian_interface_(),
      franka_velocity_cartesian_interface_(),
      position_joint_limit_interface_(),
      velocity_joint_limit_interface_(),
      effort_joint_limit_interface_(),
      publish_rate_(publish_rate),
      publisher_transforms_(node_handle, "/tf", 1),
      publisher_franka_states_(node_handle, "franka_states", 1),
      publisher_joint_states_(node_handle, "joint_states", 1),
      publisher_external_wrench_(node_handle, "F_ext", 1),
      joint_names_(),
      arm_id_(arm_id),
      robot_(robot),
      robot_state_(),
      position_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      velocity_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      effort_joint_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      pose_cartesian_command_({1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
                               1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
      velocity_cartesian_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}),
      default_run_function_([this](std::function<bool()> ros_callback) {
        robot_->read(std::bind(&FrankaHW::readCallback, this, ros_callback,
                               std::placeholders::_1));
      }),
      run_function_(default_run_function_) {
  joint_names_.resize(joint_names.size());
  joint_names_ = joint_names;

  for (size_t i = 0; i < joint_names_.size(); ++i) {
    hardware_interface::JointStateHandle joint_handle(
        joint_names_[i], &robot_state_.q_d[i], &robot_state_.dq[i],
        &robot_state_.tau_J[i]);
    joint_state_interface_.registerHandle(joint_handle);

    FrankaJointStateHandle franka_joint_handle(
        joint_names_[i], robot_state_.q[i], robot_state_.dq[i],
        robot_state_.tau_J[i], robot_state_.q_d[i], robot_state_.dtau_J[i],
        robot_state_.tau_ext_hat_filtered[i], robot_state_.joint_collision[i],
        robot_state_.joint_contact[i]);
    franka_joint_state_interface_.registerHandle(franka_joint_handle);

    hardware_interface::JointHandle position_joint_handle(
        joint_state_interface_.getHandle(joint_names_[i]),
        &position_joint_command_.q[i]);
    position_joint_interface_.registerHandle(position_joint_handle);

    hardware_interface::JointHandle velocity_joint_handle(
        joint_state_interface_.getHandle(joint_names_[i]),
        &velocity_joint_command_.dq[i]);
    velocity_joint_interface_.registerHandle(velocity_joint_handle);

{'fraction_non_alphanumeric': 0.12588832487309645, 'fraction_numerical': 0.02668600435097897, 'mean_word_length': 3.9398280802292263, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 29, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7219409', 'n_tokens_mistral': 2492, 'n_tokens_neox': 2392, 'n_words': 324}