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