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. }