void IkJointControllerClass::getParams(ros::NodeHandle& n) {
ROS_INFO("getting gains params and join name params");
XmlRpc::XmlRpcValue joint_names; //array of 7 joint names
if (!n.getParam("joints", joint_names)){
ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
}
if ((joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) || (joint_names.size() != 7)){
ROS_ERROR("Malformed joint specification. (namespace: %s)", n.getNamespace().c_str());
}
std::string joint_param_ns;
for (int i = 0; i < 7; i++){
XmlRpc::XmlRpcValue &name_value = joint_names[i];
if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString){
ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", n.getNamespace().c_str());
}
joint_names_[i] = ((std::string)name_value).c_str();
joint_param_ns = std::string("gains/") + joint_names_[i];
n.getParam(joint_param_ns + "/p", p_[i]);
n.getParam(joint_param_ns + "/d", d_[i]);
ROS_INFO("NEW YAML got %d-th joint name %s with (p,d) gains parameters: (%f,%f)",
i,
joint_names_[i].c_str(),
p_[i],
d_[i]);
}
}
void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
std::string urdf_string;
urdf_model_ = new urdf::Model();
// search and wait for robot_description on param server
while (urdf_string.empty() && ros::ok())
{
std::string search_param_name;
if (nh.searchParam(param_name, search_param_name))
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << search_param_name);
nh.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << param_name);
nh.getParam(param_name, urdf_string);
}
usleep(100000);
}
if (!urdf_model_->initString(urdf_string))
ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
else
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}
bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
// Get joint name from parameter server
std::string joint_name;
if (!n.getParam("joint", joint_name)) {
ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
return false;
}
if (!n.getParam("effort_threshold", effort_threshold)) {
ROS_ERROR("Effort threshold not given (namespace: %s)", n.getNamespace().c_str());
return false;
}
// Get joint handle from hardware interface
joint_ = robot->getHandle(joint_name);
// Load PID Controller using gains set on parameter server
if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
return false;
// Start realtime state publisher
controller_state_publisher_.reset(
new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
(n, "state", 1));
// Start command subscriber
sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
return true;
}
/// Controller initialization in non-realtime
bool MyCartControllerClass::init(pr2_mechanism_model::RobotState *robot,
ros::NodeHandle &n)
{
// Get the root and tip link names from parameter server.
std::string root_name, tip_name;
if (!n.getParam("root_name", root_name))
{
ROS_ERROR("No root name given in namespace: %s)",
n.getNamespace().c_str());
return false;
}
if (!n.getParam("tip_name", tip_name))
{
ROS_ERROR("No tip name given in namespace: %s)",
n.getNamespace().c_str());
return false;
}
// Construct a chain from the root to the tip and prepare the kinematics.
// Note the joints must be calibrated.
if (!chain_.init(robot, root_name, tip_name))
{
ROS_ERROR("MyCartController could not use the chain from '%s' to '%s'",
root_name.c_str(), tip_name.c_str());
return false;
}
// Store the robot handle for later use (to get time).
robot_state_ = robot;
// Construct the kdl solvers in non-realtime.
chain_.toKDL(kdl_chain_);
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
// Resize (pre-allocate) the variables in non-realtime.
q_.resize(kdl_chain_.getNrOfJoints());
q0_.resize(kdl_chain_.getNrOfJoints());
qdot_.resize(kdl_chain_.getNrOfJoints());
tau_.resize(kdl_chain_.getNrOfJoints());
J_.resize(kdl_chain_.getNrOfJoints());
// Pick the gains.
Kp_.vel(0) = 100.0; Kd_.vel(0) = 1.0; // Translation x
Kp_.vel(1) = 100.0; Kd_.vel(1) = 1.0; // Translation y
Kp_.vel(2) = 100.0; Kd_.vel(2) = 1.0; // Translation z
Kp_.rot(0) = 100.0; Kd_.rot(0) = 1.0; // Rotation x
Kp_.rot(1) = 100.0; Kd_.rot(1) = 1.0; // Rotation y
Kp_.rot(2) = 100.0; Kd_.rot(2) = 1.0; // Rotation z
return true;
}
void getParams(const ros::NodeHandle &nh)
{
//avg accel update period (number of msgs until next localization udpate)
if (nh.getParam("update_period", UPDATE_PERIOD))
{
ROS_INFO("Set %s/update_period to %d",nh.getNamespace().c_str(), UPDATE_PERIOD);
}
else
{
if(nh.hasParam("update_period"))
ROS_WARN("%s/update_period must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
else
ROS_WARN("No value set for %s/update_period. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
UPDATE_PERIOD = DEFAULT_LOC_NODE_UPDATE_PERIOD;
}
//frequency this node publishes a new topic
if (nh.getParam("publish_freq", PUBLISH_FREQ))
{
ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
}
else
{
if(nh.hasParam("publish_freq"))
ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
else
ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
PUBLISH_FREQ = DEFAULT_LOC_NODE_PUBLISH_FREQ;
}
//number of states from coax_filter this node will buffer before it begins to drop them
if (nh.getParam("fstate_msg_buffer", FSTATE_MSG_BUFFER))
{
ROS_INFO("Set %s/fstate_msg_buffer to %d",nh.getNamespace().c_str(), FSTATE_MSG_BUFFER);
}
else
{
if(nh.hasParam("fstate_msg_buffer"))
ROS_WARN("%s/fstate_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
else
ROS_WARN("No value set for %s/fstate_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
FSTATE_MSG_BUFFER = DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER;
}
//number of messages this node will queue for publishing before droping old data
if (nh.getParam("msg_queue", MSG_QUEUE))
{
ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
}
else
{
if(nh.hasParam("msg_queue"))
ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
else
ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
MSG_QUEUE = DEFAULT_LOC_NODE_MSG_QUEUE;
}
}
void callbackThreadFunction(SharedMemoryTransport<T>* smt, boost::function<void(T&)> callback)
{
T msg;
std::string serialized_data;
ros::Rate loop_rate(10.0);
while(ros::ok()) //wait for the field to at least have something
{
if(!smt->initialized())
{
ROS_WARN("%s: Shared memory transport was shut down while we were waiting for connections. Stopping callback thread!", m_nh->getNamespace().c_str());
return;
}
if(!smt->connected())
{
smt->connect();
}
else if(smt->hasData())
{
break;
}
ROS_WARN_STREAM_THROTTLE(1.0, m_nh->getNamespace() << ": Trying to connect to field " << smt->getFieldName() << "...");
loop_rate.sleep();
//boost::this_thread::interruption_point();
}
if(getCurrentMessage(msg))
{
callback(msg);
}
while(ros::ok())
{
try
{
if(m_use_polling)
{
smt->awaitNewDataPolled(msg);
}
else
{
smt->awaitNewData(msg);
}
callback(msg);
}
catch(ros::serialization::StreamOverrunException& ex)
{
ROS_ERROR("%s: Deserialization failed for topic %s! The string was:\n%s", m_nh->getNamespace().c_str(), m_full_topic_path.c_str(), serialized_data.c_str());
}
//boost::this_thread::interruption_point();
}
}
void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
if(_use_dynamic_reconfig)
{
// classical PID init
_pid.init(_node);
}
else
{
control_toolbox::Pid::Gains gains;
// Load PID gains from parameter server
if (!_node.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", _node.getNamespace().c_str());
return;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
_node.param("i", gains.i_gain_, 0.0);
_node.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
_node.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
_pid.setGains(gains);
}
}
static void require_param(const ros::NodeHandle &nh, const std::string ¶m_name, T &var)
{
if(!nh.getParam(param_name, var)) {
ROS_FATAL_STREAM("Required parameter not found! Namespace: "<<nh.getNamespace()<<" Parameter: "<<param_name);
throw ros::InvalidParameterException("Parameter not found!");
}
}
void publishObstacleMarker(const pcl::PointXYZ& obstacle_location)
{
if (marker_pub.getNumSubscribers())
{
visualization_msgs::Marker obstacle;
obstacle.header.frame_id = nh.getNamespace() + "/kinect_depth";
obstacle.header.stamp = ros::Time::now();
obstacle.ns = "obstacle";
obstacle.action = visualization_msgs::Marker::ADD;
obstacle.pose.position.x = obstacle_location.x;
obstacle.pose.position.y = obstacle_location.y;
obstacle.pose.position.z = obstacle_location.z;
obstacle.pose.orientation.w = 1.0;
obstacle.id = 0;
obstacle.type = visualization_msgs::Marker::SPHERE;
obstacle.scale.x = 0.1;
obstacle.scale.y = 0.1;
obstacle.scale.z = 0.1;
obstacle.color.r = 1.0;
obstacle.color.g = 0.0;
obstacle.color.b = 0.0;
obstacle.color.a = 0.8;
obstacle.lifetime = ros::Duration(1.0);
marker_pub.publish(obstacle);
}
}
void callback(const giskard_msgs::ControllerFeedback::ConstPtr& msg)
{
visualization_msgs::MarkerArray out_msg;
// TODO: get this from the server or a callback
out_msg.markers = to_markers(msg->current_command, 0.1, nh_.getNamespace());
pub_.publish(out_msg);
}
ModelStatesWatcher::ModelStatesWatcher( ros::NodeHandle &nh, std::string topic_name )
: valid_mspose(false), model_name(nh.getNamespace()), near_ego_vehicle(true),
mssub(nh.subscribe( topic_name, 1, &ModelStatesWatcher::ms_cb, this ))
{
while (model_name[0] == '/')
model_name = std::string(model_name.c_str()+1);
}
void publishDiagnostics()
{
// publishing diagnotic messages
diagnostic_msgs::DiagnosticArray diagnostics;
diagnostics.status.resize(1);
diagnostics.status[0].name = nh_.getNamespace();
diagnostics.status[0].values.resize(1);
diagnostics.status[0].values[0].key = "error_count";
diagnostics.status[0].values[0].value = boost::lexical_cast<std::string>( error_counter_);
// set data to diagnostics
if (isDSAInitialized_)
{
diagnostics.status[0].level = 0;
diagnostics.status[0].message = "DSA tactile sensing initialized and running";
}
else if(error_counter_ == 0)
{
diagnostics.status[0].level = 1;
diagnostics.status[0].message = "DSA not initialized";
}
else
{
diagnostics.status[0].level = 2;
diagnostics.status[0].message = "DSA exceeded eror count";
}
// publish diagnostic message
topicPub_Diagnostics_.publish(diagnostics);
if(debug_) ROS_DEBUG_STREAM("publishDiagnostics " << diagnostics);
}
FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
as_(nh, nh.getNamespace(),
boost::bind(&FootstepPlanner::planCB, this, _1), false)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
srv_->setCallback (f);
pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
"text", 1, true);
pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"close_list", 1, true);
pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"open_list", 1, true);
srv_project_footprint_ = nh.advertiseService(
"project_footprint", &FootstepPlanner::projectFootPrintService, this);
srv_project_footprint_with_local_search_ = nh.advertiseService(
"project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
{
boost::mutex::scoped_lock lock(mutex_);
if (!readSuccessors(nh)) {
return;
}
JSK_ROS_INFO("building graph");
buildGraph();
JSK_ROS_INFO("build graph done");
}
sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
as_.start();
}
void publishError(std::string error_str) {
diagnostic_msgs::DiagnosticArray diagnostics;
diagnostics.status.resize(1);
diagnostics.status[0].level = 2;
diagnostics.status[0].name = nh.getNamespace();
diagnostics.status[0].message = error_str;
topicPub_Diagnostic_.publish(diagnostics);
}
Rigid_Body(ros::NodeHandle& nh, std::string server_ip,
int port)
{
target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
std::string connec_nm = server_ip + ":" + boost::lexical_cast<std::string>(port);
connection = vrpn_get_connection_by_name(connec_nm.c_str());
std::string target_name = nh.getNamespace().substr(1);
tracker = new vrpn_Tracker_Remote(target_name.c_str(), connection);
this->tracker->register_change_handler(NULL, track_target);
}
void publishObstacle(const bool obstacle_found, const pcl::PointXYZ& location)
{
starmac_kinect::Obstacle obs_msg;
obs_msg.header.stamp = ros::Time::now();
obs_msg.header.frame_id = nh.getNamespace() + "/kinect_depth";
obs_msg.obstacle_found = obstacle_found;
obs_msg.location.x = location.x;
obs_msg.location.y = location.y;
obs_msg.location.z = location.z;
obstacle_pub.publish(obs_msg);
}
bool retrieveParameter(const ros::NodeHandle& n, const std::string& ns, const std::string& param_name, double& param)
{
std::string full_name = construct_full_name(ns, param_name);
// look up parameter from server
if(!n.getParam(full_name, param))
{
ROS_ERROR("Parameter '%s' not given in namespace: '%s'", full_name.c_str(), n.getNamespace().c_str());
return false;
}
return true;
}
ExploitationMatcher::ExploitationMatcher(
ros::NodeHandle& n, ros::NodeHandle& np)
{
sub_intention_ = n.subscribe("intention", 10,
&ExploitationMatcher::intentionCB, this);
sub_priority_ = np.subscribe("priority", 10,
&ExploitationMatcher::priorityCB, this);
pub_desire_ =
np.advertise<std_msgs::String>("exploited_desire", 10);
srv_register_ =
np.advertiseService("register_em", &ExploitationMatcher::registerCB, this);
topic_ns_ = np.getNamespace();
}
void ParameterCatalog::fetch(ros::NodeHandle nh){
ROS_INFO_NAMED(CONFIG_LOG, "fetching parameters from namespace %s",
nh.getNamespace().c_str());
m_nodehandle = nh;
// TODO clean this up, setmotionprimitive needs to be run before parse
// stuff!
setMotionPrimitiveParams(m_motion_primitive_params);
setRobotResolutionParams(m_motion_primitive_params,
m_robot_resolution_params);
setOccupancyGridParams(m_occupancy_grid_params);
setLeftArmParams(m_left_arm_params);
setRightArmParams(m_right_arm_params);
}
// Initialize.
bool UAVOdometry::Initialize(const ros::NodeHandle& n) {
name_ = ros::names::append(n.getNamespace(), "uav_odometry");
if (!LoadParameters(n)) {
ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
return false;
}
if (!RegisterCallbacks(n)) {
ROS_ERROR("%s: Failed to register callbacks.", name_.c_str());
return false;
}
return true;
}
void Pid::initDynamicReconfig(ros::NodeHandle &node)
{
ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
<< node.getNamespace());
// Start dynamic reconfigure server
param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
dynamic_reconfig_initialized_ = true;
// Set Dynamic Reconfigure's gains to Pid's values
updateDynamicReconfig();
// Set callback
param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
param_reconfig_server_->setCallback(param_reconfig_callback_);
}
// Initialize.
bool UAVLocalization::Initialize(const ros::NodeHandle& n,
UAVMapper *mapper, UAVOdometry *odometry) {
name_ = ros::names::append(n.getNamespace(), "uav_localization");
odometry_ = odometry;
mapper_ = mapper;
if (!LoadParameters(n)) {
ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
return false;
}
if (!RegisterCallbacks(n)) {
ROS_ERROR("%s: Failed to register callbacks.", name_.c_str());
return false;
}
initialized_ = true;
return true;
}
bool JointPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
std::string joint_name;
if (!n.getParam("joint", joint_name)) {
ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
return false;
}
control_toolbox::Pid pid;
if (!pid.init(ros::NodeHandle(n, "pid")))
return false;
controller_state_publisher_.reset(
new realtime_tools::RealtimePublisher<controllers_msgs::JointControllerState>(n, "state", 1));
sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
return init(robot, joint_name, pid);
}
bool SimpleController::init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n)
{
lcm_ = boost::shared_ptr<lcm::LCM>(new lcm::LCM);
if (!lcm_->good())
{
std::cerr << "ERROR: lcm is not good()" << std::endl;
return false;
}
lcm_->subscribe("VAL_TRANSLATOR_JOINT_COMMAND", &SimpleController::jointCommandHandler, this);
effortJointHandles.clear();
std::vector<std::string> jointNames;
if(n.getParam("joints", jointNames))
{
if(hw)
{
for(unsigned int i=0; i < jointNames.size(); ++i)
{
effortJointHandles.push_back(hw->getHandle(jointNames[i]));
}
buffer_command_efforts.resize(effortJointHandles.size(), 0.0);
buffer_current_positions.resize(effortJointHandles.size(), 0.0);
buffer_current_velocities.resize(effortJointHandles.size(), 0.0);
buffer_current_efforts.resize(effortJointHandles.size(), 0.0);
}
else
{
ROS_ERROR("Effort Joint Interface is empty in hardware interace.");
return false;
}
}
else
{
ROS_ERROR("No joints in given namespace: %s", n.getNamespace().c_str());
return false;
}
return true;
}
/// Controller initialization in non-realtime
bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
ros::NodeHandle &n)
{
std::string joint_name;
if (!n.getParam("joint_name", joint_name))
{
ROS_ERROR("No joint given in namespace: '%s')",
n.getNamespace().c_str());
return false;
}
joint_state_ = robot->getJointState(joint_name);
if (!joint_state_)
{
ROS_ERROR("MyController could not find joint named '%s'",
joint_name.c_str());
return false;
}
return true;
}
void getParams(const ros::NodeHandle &nh)
{
//frequency this node publishes a new topic
if(nh.getParam("publish_freq", PUBLISH_FREQ))
{
ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
}
else
{
if(nh.hasParam("publish_freq"))
ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ);
else
ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ);
PUBLISH_FREQ = DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ;
}
//number of blob sequences from blob_filter this node will buffer before it begins to drop them
if (nh.getParam("sequence_poses_msg_buffer", SEQ_POSES_MSG_BUFFER))
{
ROS_INFO("Set %s/sequence_poses_msg_buffer to %d",nh.getNamespace().c_str(), SEQ_POSES_MSG_BUFFER);
}
else
{
if(nh.hasParam("sequences_msg_buffer"))
ROS_WARN("%s/sequences_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
else
ROS_WARN("No value set for %s/sequences_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
SEQ_POSES_MSG_BUFFER = DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER;
}
//number of messages this node will queue for publishing before it drops data
if (nh.getParam("msg_queue", MSG_QUEUE))
{
ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
}
else
{
if(nh.hasParam("msg_queue"))
ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE);
else
ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE);
MSG_QUEUE = DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE;
}
}
bool JointPositionsController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
// Get joint name from parameter server
std::string joint_name;
if (!n.getParam("joint", joint_name))
{
ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
return false;
}
// Load PID Controller using gains set on parameter server
pid_controller_.reset(new control_toolbox::Pid());
if (!pid_controller_->init(ros::NodeHandle(n, "pid")))
return false;
// Start realtime state publisher
controller_state_publisher_.reset(
new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1));
// Start command subscriber
sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionsController::setCommandCB, this);
// Get joint handle from hardware interface
joint_ = robot->getHandle(joint_name);
// Get URDF info about joint
urdf::Model urdf;
if (!urdf.initParam("robot_description"))
{
ROS_ERROR("Failed to parse urdf file");
return false;
}
joint_urdf_ = urdf.getJoint(joint_name);
if (!joint_urdf_)
{
ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
return false;
}
return true;
}
/// Controller initialization in non-realtime
bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
ros::NodeHandle &n)
{
// get joint name
std::string joint_name;
if (!n.getParam("joint_name", joint_name))
{
ROS_ERROR("No joint given in namespace: '%s')",
n.getNamespace().c_str());
return false;
}
// get pointer to joint state
joint_state_ = robot->getJointState(joint_name);
if (!joint_state_)
{
ROS_ERROR("MyController could not find joint named '%s'",
joint_name.c_str());
return false;
}
// advertise service to set amplitude
amplitude_ = 0.5;
srv_ = n.advertiseService("set_amplitude",
&MyControllerClass::setAmplitude, this);
// copy robot pointer so we can access time
robot_ = robot;
// construct pid controller
if (!pid_controller_.init(ros::NodeHandle(n, "pid_parameters"))) {
ROS_ERROR("MyController could not construct PID controller for joint '%s'",
joint_name.c_str());
return false;
}
return true;
}
bool GroupCommandController::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n)
{
#if TRACE_GroupCommandController_ACTIVATED
ROS_INFO("GroupCommandController: Start init of robot %s !",robot_namespace_.c_str());
#endif
robot_namespace_ = n.getNamespace();
if( !(KinematicChainControllerBase<hardware_interface::PositionJointInterface>::init(robot, n)) )
{
ROS_ERROR("GroupCommandController: Couldn't initilize GroupCommandController controller of robot %s!",robot_namespace_.c_str());
return false;
}
commands_buffer_.resize(joint_handles_.size());
goal_factor_.clear();
goal_buffer_.clear();
// get joint positions, velocity
for (int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
// set initial value of desired state
joint_des_states_.q(i) = joint_msr_states_.q(i);
// set initial desired command values
commands_buffer_[i] = 0.0;
}
sub_command_ = nh_.subscribe("command", 1, &GroupCommandController::commandCB, this);
cmd_flag_ = 0; // set this flag to 0 to not to run the update method
return true;
}
bool Chain::init(ros::NodeHandle& n)
{
ROS_INFO_STREAM_NAMED("chain","The chain is initialing");
std::string robot_description;
if (!ros::param::search(n.getNamespace(),"robot_description", robot_description)){
ROS_ERROR_STREAM("Chain: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
return false;
}
// Construct an URDF model from the xml string
urdf::Model urdf_model;
urdf_model.initParam(robot_description);
// Get a KDL tree from the robot URDF
ROS_INFO_STREAM_NAMED("chain","The urdf is initialed succussfully");
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree_)){
ROS_ERROR("Could not convert urdf into kdl tree");
return false;
}
return true;
}