bool JointTrajectoryController::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n) { // Store nodehandle nh_ = n; // Get joint names XmlRpc::XmlRpcValue xml_array; if (!nh_.getParam("joint_names", xml_array)) { ROS_ERROR("No 'joint_names' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's an array type if (xml_array.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("The 'joint_names' parameter is not an array (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_array.size(); ROS_INFO_STREAM("Initializing JointTrajectoryController with " << n_joints_ << " joints."); // Get trajectory sampling resolution if (!nh_.hasParam("sampling_resolution")) { ROS_INFO("No sampling_resolution specified (namespace: %s), using default.", nh_.getNamespace().c_str()); } nh_.param("sampling_resolution", sampling_resolution_, 0.001); // Create trajectory generator rml_.reset(new ReflexxesAPI(n_joints_, sampling_resolution_)); rml_in_.reset(new RMLPositionInputParameters(n_joints_)); rml_out_.reset(new RMLPositionOutputParameters(n_joints_)); // Get urdf urdf::Model urdf; std::string urdf_str; ros::NodeHandle nh; nh.getParam("/robot_description", urdf_str); if (!urdf.initString(urdf_str)) { ROS_ERROR("Failed to parse urdf from '/robot_description' parameter (namespace: %s)", nh.getNamespace().c_str()); return false; } // Get individual joint properties from urdf and parameter server joint_names_.resize(n_joints_); joints_.resize(n_joints_); urdf_joints_.resize(n_joints_); position_tolerances_.resize(n_joints_); max_accelerations_.resize(n_joints_); max_jerks_.resize(n_joints_); commanded_positions_.resize(n_joints_); for (int i = 0; i < n_joints_; i++) { // Get joint name if (xml_array[i].getType() != XmlRpc::XmlRpcValue::TypeString) { ROS_ERROR("The 'joint_names' parameter contains a non-string element (namespace '%s')", nh_.getNamespace().c_str()); return false; } joint_names_[i] = static_cast<std::string>(xml_array[i]); // Get the joint-namespace nodehandle { ros::NodeHandle joint_nh(nh_, "joints/" + joint_names_[i]); ROS_INFO("Loading joint information for joint '%s' (namespace: %s)", joint_names_[i].c_str(), joint_nh.getNamespace().c_str()); // Get position tolerance if (!joint_nh.hasParam("position_tolerance")) { ROS_INFO("No position_tolerance specified (namespace: %s), using default.", joint_nh.getNamespace().c_str()); } joint_nh.param("position_tolerance", position_tolerances_[i], 0.1); // Get maximum acceleration if (!joint_nh.hasParam("max_acceleration")) { ROS_INFO("No max_acceleration specified (namespace: %s), using default.", joint_nh.getNamespace().c_str()); } joint_nh.param("max_acceleration", max_accelerations_[i], 1.0); // Get maximum acceleration if (!joint_nh.hasParam("max_jerk")) { ROS_INFO("No max_jerk specified (namespace: %s), using default.", joint_nh.getNamespace().c_str()); } joint_nh.param("max_jerk", max_jerks_[i], 1000.0); } // Get ros_control joint handle joints_[i] = robot->getHandle(joint_names_[i]); // Get urdf joint urdf_joints_[i] = urdf.getJoint(joint_names_[i]); if (!urdf_joints_[i]) { ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str()); return false; } // Get RML parameters from URDF rml_in_->MaxVelocityVector->VecData[i] = urdf_joints_[i]->limits->velocity; rml_in_->MaxAccelerationVector->VecData[i] = max_accelerations_[i]; rml_in_->MaxJerkVector->VecData[i] = max_jerks_[i]; } for (int i = 0; i < n_joints_; i++) { rml_in_->SelectionVector->VecData[i] = true; } if (rml_in_->CheckForValidity()) { ROS_INFO_STREAM("RML INPUT Configuration Valid."); this->rml_debug(ros::console::levels::Debug); } else { ROS_ERROR_STREAM("RML INPUT Configuration Invalid!"); this->rml_debug(ros::console::levels::Warn); return false; } // Create state publisher // TODO: create state publisher //controller_state_publisher_.reset( //new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1)); // Create command subscriber trajectory_command_sub_ = nh_.subscribe<trajectory_msgs::JointTrajectory>( "trajectory_command", 1, &JointTrajectoryController::trajectoryCommandCB, this); return true; }
bool BaxterEffortController::init( hardware_interface::EffortJointInterface *robot, ros::NodeHandle &nh) { // Store nodehandle nh_ = nh; // Get joint sub-controllers XmlRpc::XmlRpcValue xml_struct; if( !nh_.getParam("joints", xml_struct) ) { ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's a struct if(xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_struct.size(); ROS_INFO_STREAM("Initializing BaxterEffortController with "<<n_joints_<<" joints."); effort_controllers_.resize(n_joints_); int i = 0; // track the joint id for(XmlRpc::XmlRpcValue::iterator joint_it = xml_struct.begin(); joint_it != xml_struct.end(); ++joint_it) { // Get joint controller if(joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); return false; } // Get joint names from the parameter server std::string joint_name[n_joints_]; // Get joint controller name std::string joint_controller_name = joint_it->first; // Get the joint-namespace nodehandle { ros::NodeHandle joint_nh(nh_, "joints/"+joint_controller_name); ROS_INFO_STREAM_NAMED("init","Loading sub-controller '" << joint_controller_name << "', Namespace: " << joint_nh.getNamespace()); effort_controllers_[i].reset(new effort_controllers::JointEffortController()); effort_controllers_[i]->init(robot, joint_nh); if( !joint_nh.getParam("joint",joint_name[i])) { ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", joint_nh.getNamespace().c_str()); return false; } // Create a publisher for every joint controller that publishes to the command topic under that controller effort_command_pub_[i]=joint_nh.advertise<std_msgs::Float64>("command",1); } // end of joint-namespaces // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) joint_to_index_map_.insert(std::pair<std::string,std::size_t> (joint_name[i],i)); // increment joint i ++i; } // Get controller topic name that it will subscribe to if( nh_.getParam("topic", topic_name) ) // They provided a custom topic to subscribe to { // Get a node handle that is relative to the base path ros::NodeHandle nh_base("~"); // Create command subscriber custom to baxter effort_command_sub_ = nh_base.subscribe<baxter_core_msgs::JointCommand> (topic_name, 1, &BaxterEffortController::commandCB, this); } else // default "command" topic { // Create command subscriber custom to baxter effort_command_sub_ = nh_.subscribe<baxter_core_msgs::JointCommand> ("command", 1, &BaxterEffortController::commandCB, this); } return true; }
bool JointPositionController::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n) { // Store nodehandle nh_ = n; // Get joint names XmlRpc::XmlRpcValue xml_array; if (!nh_.getParam("joint_names", xml_array)) { ROS_ERROR("No 'joint_names' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Make sure it's an array type if (xml_array.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("The 'joint_names' parameter is not an array (namespace '%s')", nh_.getNamespace().c_str()); return false; } // Get number of joints n_joints_ = xml_array.size(); ROS_INFO_STREAM("Initializing JointPositionController with " << n_joints_ << " joints."); // Get trajectory sampling resolution if (!nh_.hasParam("sampling_resolution")) { ROS_INFO("No sampling_resolution specified (namespace: %s), using default.", nh_.getNamespace().c_str()); } nh_.param("sampling_resolution", sampling_resolution_, DEFAULT_SAMPLING_RESOLUTION); // Get behavior after reaching point if (!nh_.hasParam("recompute_trajectory")) { ROS_INFO("No behavior after reaching point specified (namespace: %s), using default (keep trajectory).", nh_.getNamespace().c_str()); } nh_.param("recompute_trajectory", recompute_trajectory_, false); // Get minimum synchronization time if (!nh_.hasParam("minimum_synchronization_time")) { ROS_INFO("No minimum synchronization time specified (namespace: %s), using default (%f).", nh_.getNamespace().c_str(), DEFAULT_MIN_SYNCHRONIZATION_TIME); } nh_.param("minimum_synchronization_time", minimum_synchronization_time_, DEFAULT_MIN_SYNCHRONIZATION_TIME); // Get position tolerance if (!nh_.hasParam("command_update_tolerance")) { ROS_INFO("No command_update_tolerance specified (namespace: %s), using default (%f).", nh_.getNamespace().c_str(), DEFAULT_COMMAND_UPDATE_TOLERANCE); } nh_.param("command_update_tolerance", command_update_tolerance_, DEFAULT_COMMAND_UPDATE_TOLERANCE); ROS_INFO("Using command update tolerance %f", command_update_tolerance_); // Create trajectory generator rml_.reset(new ReflexxesAPI(n_joints_, sampling_resolution_)); rml_in_.reset(new RMLPositionInputParameters(n_joints_)); rml_out_.reset(new RMLPositionOutputParameters(n_joints_)); // Get urdf urdf::Model urdf; std::string urdf_str; ros::NodeHandle nh; nh.getParam("/robot_description", urdf_str); if (!urdf.initString(urdf_str)) { ROS_ERROR("Failed to parse urdf from '/robot_description' parameter (namespace: %s)", nh.getNamespace().c_str()); return false; } // Get individual joint properties from urdf and parameter server joint_names_.resize(n_joints_); joints_.resize(n_joints_); urdf_joints_.resize(n_joints_); position_tolerances_.resize(n_joints_); max_velocities_.resize(n_joints_); max_accelerations_.resize(n_joints_); previous_positions_.resize(n_joints_); previous_velocities_.resize(n_joints_); current_velocities_.resize(n_joints_); current_accelerations_.resize(n_joints_); max_jerks_.resize(n_joints_); commanded_positions_.resize(n_joints_); for (int i = 0; i < n_joints_; i++) { // Get joint name if (xml_array[i].getType() != XmlRpc::XmlRpcValue::TypeString) { ROS_ERROR("The 'joint_names' parameter contains a non-string element (namespace '%s')", nh_.getNamespace().c_str()); return false; } joint_names_[i] = static_cast<std::string>(xml_array[i]); // Get the joint-namespace nodehandle { ros::NodeHandle joint_nh(nh_, "joints/" + joint_names_[i]); ROS_INFO("Loading joint information for joint '%s' (namespace: %s)", joint_names_[i].c_str(), joint_nh.getNamespace().c_str()); // Get position tolerance if (!joint_nh.hasParam("tracking_position_tolerance")) { ROS_INFO("No tracking_position_tolerance specified (namespace: %s), using default (%f).", joint_nh.getNamespace().c_str(), DEFAULT_POSITION_TOLERANCE); } joint_nh.param("tracking_position_tolerance", position_tolerances_[i], DEFAULT_POSITION_TOLERANCE); ROS_INFO("Using tolerance %f for joint %d.", position_tolerances_[i], i); // Get maximum velocity if (!joint_nh.hasParam("max_velocity")) { ROS_INFO("No max_velocity specified (namespace: %s), using default from URDF.", joint_nh.getNamespace().c_str()); } joint_nh.param("max_velocity", max_velocities_[i], 0.0); // default will be replaced later // Get maximum acceleration if (!joint_nh.hasParam("max_acceleration")) { ROS_INFO("No max_acceleration specified (namespace: %s), using default.", joint_nh.getNamespace().c_str()); } joint_nh.param("max_acceleration", max_accelerations_[i], DEFAULT_MAX_ACCELERATION); // Get maximum jerk if (!joint_nh.hasParam("max_jerk")) { ROS_INFO("No max_jerk specified (namespace: %s), using default.", joint_nh.getNamespace().c_str()); } joint_nh.param("max_jerk", max_jerks_[i], DEFAULT_MAX_JERK); } // Get ros_control joint handle joints_[i] = robot->getHandle(joint_names_[i]); // Get urdf joint urdf_joints_[i] = urdf.getJoint(joint_names_[i]); if (!urdf_joints_[i]) { ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str()); return false; } // Get RML parameters from URDF rml_in_->MaxVelocityVector->VecData[i] = max_velocities_[i] == 0 ? urdf_joints_[i]->limits->velocity : max_velocities_[i]; rml_in_->MaxAccelerationVector->VecData[i] = max_accelerations_[i]; rml_in_->MaxJerkVector->VecData[i] = max_jerks_[i]; } for (int i = 0; i < n_joints_; i++) { rml_in_->SelectionVector->VecData[i] = true; } if (rml_in_->CheckForValidity()) { ROS_INFO_STREAM("RML INPUT Configuration Valid."); this->rml_debug(ros::console::levels::Debug); } else { ROS_ERROR_STREAM("RML INPUT Configuration Invalid!"); this->rml_debug(ros::console::levels::Warn); return false; } // Create state publisher // TODO: create state publisher //controller_state_publisher_.reset( //new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1)); // Reset commands for (int i = 0; i < n_joints_; i++) { commanded_positions_[i] = joints_[i].getPosition(); } // Create command subscriber trajectory_command_sub_ = nh_.subscribe<trajectory_msgs::JointTrajectoryPoint>( "joint_position_command", 1, &JointPositionController::trajectoryCommandCB, this); return true; }