void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
void rosNamed(const std::vector<std::string> &msgs) {
if (msgs.size()==0) return;
if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); }
if (msgs.size()==2) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); }
}
if (msgs.size()==3) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
}
}
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
// Save pointers to the model
parent_model_ = parent;
sdf_ = sdf;
// Error message if the model couldn't be found
if (!parent_model_)
{
ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
return;
}
// Check that ROS has been initialized
if(!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// Get namespace for nodehandle
if(sdf_->HasElement("robotNamespace"))
{
robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
}
else
{
robot_namespace_ = parent_model_->GetName(); // default
}
// Get robot_description ROS param name
if (sdf_->HasElement("robotParam"))
{
robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
}
else
{
robot_description_ = "robot_description"; // default
}
// Get the robot simulation interface type
if(sdf_->HasElement("robotSimType"))
{
robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
}
else
{
robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
}
// Get the Gazebo simulation period
ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
// Decide the plugin control period
if(sdf_->HasElement("controlPeriod"))
{
control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
// Check the period against the simulation period
if( control_period_ < gazebo_period )
{
ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
}
else if( control_period_ > gazebo_period )
{
ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
}
}
else
{
control_period_ = gazebo_period;
ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
<< control_period_);
}
// Get parameters/settings for controllers from ROS param server
model_nh_ = ros::NodeHandle(robot_namespace_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
if (sdf_->HasElement("eStopTopic"))
{
const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this);
}
ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
const std::string urdf_string = getURDF(robot_description_);
if (!parseTransmissionsFromURDF(urdf_string))
{
ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
return;
}
// Load the RobotHWSim abstraction to interface the controllers with the gazebo model
try
{
robot_hw_sim_loader_.reset
(new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
("gazebo_ros_control",
"gazebo_ros_control::RobotHWSim"));
robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
urdf::Model urdf_model;
const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
{
ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
return;
}
// Create the controller manager
ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
controller_manager_.reset
(new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
// Listen to the update event. This event is broadcast every simulation iteration.
update_connection_ =
gazebo::event::Events::ConnectWorldUpdateBegin
(boost::bind(&GazeboRosControlPlugin::Update, this));
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
}
ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIMU::LoadThread()
{
// load parameters
this->robot_namespace_ = "";
if (this->sdf->HasElement("robotNamespace"))
this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
if (!this->sdf->HasElement("serviceName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <serviceName>, defaults to /default_imu");
this->service_name_ = "/default_imu";
}
else
this->service_name_ = this->sdf->Get<std::string>("serviceName");
if (!this->sdf->HasElement("topicName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <topicName>, defaults to /default_imu");
this->topic_name_ = "/default_imu";
}
else
this->topic_name_ = this->sdf->Get<std::string>("topicName");
if (!this->sdf->HasElement("gaussianNoise"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0.0;
}
else
this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
if (!this->sdf->HasElement("bodyName"))
{
ROS_FATAL_NAMED("imu", "imu plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = this->sdf->Get<std::string>("bodyName");
if (!this->sdf->HasElement("xyzOffset"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <xyzOffset>, defaults to 0s");
this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
}
else
this->offset_.Pos() = this->sdf->Get<ignition::math::Vector3d>("xyzOffset");
if (!this->sdf->HasElement("rpyOffset"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <rpyOffset>, defaults to 0s");
this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0));
}
else
this->offset_.Rot() = ignition::math::Quaterniond(this->sdf->Get<ignition::math::Vector3d>("rpyOffset"));
if (!this->sdf->HasElement("updateRate"))
{
ROS_DEBUG_NAMED("imu", "imu plugin missing <updateRate>, defaults to 0.0"
" (as fast as possible)");
this->update_rate_ = 0.0;
}
else
this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();
if (!this->sdf->HasElement("frameName"))
{
ROS_INFO_NAMED("imu", "imu plugin missing <frameName>, defaults to <bodyName>");
this->frame_name_ = link_name_;
}
else
this->frame_name_ = this->sdf->Get<std::string>("frameName");
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("imu", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// publish multi queue
this->pmq.startServiceThread();
// assert that the body by link_name_ exists
this->link = boost::dynamic_pointer_cast<physics::Link>(
this->world_->GetEntity(this->link_name_));
if (!this->link)
{
ROS_FATAL_NAMED("imu", "gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
this->link_name_.c_str());
return;
}
// if topic name specified as empty, do not publish
if (this->topic_name_ != "")
{
this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
this->topic_name_, 1);
// advertise services on the custom queue
ros::AdvertiseServiceOptions aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
this->srv_ = this->rosnode_->advertiseService(aso);
}
// Initialize the controller
this->last_time_ = this->world_->GetSimTime();
// this->initial_pose_ = this->link->GetPose();
this->last_vpos_ = this->link->GetWorldLinearVel().Ign();
this->last_veul_ = this->link->GetWorldAngularVel().Ign();
this->apos_ = 0;
this->aeul_ = 0;
// start custom queue for imu
this->callback_queue_thread_ =
boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosIMU::UpdateChild, this));
}
// Constructor
PickPlaceMoveItServer(const std::string name):
nh_("~"),
movegroup_action_("pickup", true),
clam_arm_client_("clam_arm", true),
ee_marker_is_loaded_(false),
block_published_(false),
action_server_(name, false)
{
base_link_ = "/base_link";
// -----------------------------------------------------------------------------------------------
// Adding collision objects
collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1);
// -----------------------------------------------------------------------------------------------
// Connect to move_group/Pickup action server
while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server");
}
// ---------------------------------------------------------------------------------------------
// Connect to ClamArm action server
while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server");
}
// ---------------------------------------------------------------------------------------------
// Create planning scene monitor
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));
if (planning_scene_monitor_->getPlanningScene())
{
//planning_scene_monitor_->startWorldGeometryMonitor();
//planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
//planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
}
else
{
ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured");
}
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_));
// ---------------------------------------------------------------------------------------------
// Register the goal and preempt callbacks
action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this));
action_server_.start();
// Announce state
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready.");
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command...");
// ---------------------------------------------------------------------------------------------
// Send home
ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home");
clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET;
clam_arm_client_.sendGoal(clam_arm_goal_);
while(!clam_arm_client_.getState().isDone() && ros::ok())
ros::Duration(0.1).sleep();
// ---------------------------------------------------------------------------------------------
// Send fake command
fake_goalCB();
}
bool DefaultRobotHWSim::initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
// parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace);
// Resize vectors to our DOF
n_dof_ = transmissions.size();
joint_names_.resize(n_dof_);
joint_types_.resize(n_dof_);
joint_lower_limits_.resize(n_dof_);
joint_upper_limits_.resize(n_dof_);
joint_effort_limits_.resize(n_dof_);
joint_control_methods_.resize(n_dof_);
pid_controllers_.resize(n_dof_);
joint_position_.resize(n_dof_);
joint_velocity_.resize(n_dof_);
joint_effort_.resize(n_dof_);
joint_effort_command_.resize(n_dof_);
joint_position_command_.resize(n_dof_);
joint_velocity_command_.resize(n_dof_);
// Initialize values
for(unsigned int j=0; j < n_dof_; j++)
{
// Check that this transmission has one joint
if(transmissions[j].joints_.size() == 0)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has no associated joints.");
continue;
}
else if(transmissions[j].joints_.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has more than one joint. Currently the default robot hardware simulation "
<< " interface only supports one.");
continue;
}
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if (joint_interfaces.empty() &&
!(transmissions[j].actuators_.empty()) &&
!(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
{
// TODO: Deprecate HW interface specification in actuators in ROS J
joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
"The transmission will be properly loaded, but please update " <<
"your robot model to remain compatible with future versions of the plugin.");
}
if (joint_interfaces.empty())
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
"Not adding it to the robot hardware simulation.");
continue;
}
else if (joint_interfaces.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
"Currently the default robot hardware simulation interface only supports one. Using the first entry!");
//continue;
}
// Add data from transmission
joint_names_[j] = transmissions[j].joints_[0].name_;
joint_position_[j] = 1.0;
joint_velocity_[j] = 0.0;
joint_effort_[j] = 1.0; // N/m for continuous joints
joint_effort_command_[j] = 0.0;
joint_position_command_[j] = 0.0;
joint_velocity_command_[j] = 0.0;
const std::string& hardware_interface = joint_interfaces.front();
// Debug
ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
<< "' of type '" << hardware_interface << "'");
// Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// Decide what kind of command interface this actuator/joint has
hardware_interface::JointHandle joint_handle;
if(hardware_interface == "EffortJointInterface")
{
// Create effort joint interface
joint_control_methods_[j] = EFFORT;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
ej_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "PositionJointInterface")
{
// Create position joint interface
joint_control_methods_[j] = POSITION;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
pj_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "VelocityJointInterface")
{
// Create velocity joint interface
joint_control_methods_[j] = VELOCITY;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_velocity_command_[j]);
vj_interface_.registerHandle(joint_handle);
}
else
{
ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
<< hardware_interface );
return false;
}
// Get the gazebo joint that corresponds to the robot joint.
//ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
// << joint_names_[j]);
gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
if (!joint)
{
ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
<< "\" which is not in the gazebo model.");
return false;
}
sim_joints_.push_back(joint);
registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
joint_limit_nh, urdf_model,
&joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
&joint_effort_limits_[j]);
if (joint_control_methods_[j] != EFFORT)
{
// Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
// joint->SetVelocity() to control the joint.
const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" +
joint_names_[j]);
if (pid_controllers_[j].init(nh, true))
{
switch (joint_control_methods_[j])
{
case POSITION:
joint_control_methods_[j] = POSITION_PID;
break;
case VELOCITY:
joint_control_methods_[j] = VELOCITY_PID;
break;
}
}
else
{
// joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are
// going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is
// going to be called.
//joint->SetMaxForce(0, joint_effort_limits_[j]);
joint->SetParam("max_force", 0, joint_effort_limits_[j]);
}
}
}
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&ej_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
return true;
}
void GenericHWInterface::init()
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0)
{
ROS_FATAL_STREAM_NAMED("generic_hw_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace() << "/hardware_interface/joints");
exit(-1);
}
num_joints_ = joint_names_.size();
// Status
joint_position_.resize(num_joints_, 0.0);
joint_velocity_.resize(num_joints_, 0.0);
joint_effort_.resize(num_joints_, 0.0);
// Command
joint_position_command_.resize(num_joints_, 0.0);
joint_velocity_command_.resize(num_joints_, 0.0);
joint_effort_command_.resize(num_joints_, 0.0);
// Limits
joint_position_lower_limits_.resize(num_joints_, 0.0);
joint_position_upper_limits_.resize(num_joints_, 0.0);
joint_velocity_limits_.resize(num_joints_, 0.0);
joint_effort_limits_.resize(num_joints_, 0.0);
// Initialize interfaces for each joint
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Loading joint name: " << joint_names_[joint_id]);
// Create joint state interface
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id]));
// Add command interfaces to joints
// TODO: decide based on transmissions?
hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]);
position_joint_interface_.registerHandle(joint_handle_position);
hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]);
velocity_joint_interface_.registerHandle(joint_handle_velocity);
hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]);
effort_joint_interface_.registerHandle(joint_handle_effort);
// Load the joint limits
registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id);
} // end for each joint
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&effort_joint_interface_); // From RobotHW base class.
}
