NodeHandle PlugHandle::getNode() const
{
#ifdef QT_DEBUG
Q_ASSERT(m_isValid);
#else
if(!m_isValid){
return NodeHandle();
}
#endif
return NodeHandle(m_plug->getNode());
}
NodeHandles NodeHandle::get_children() const {
try {
NodeIDs children = shared_->get_children(node_);
NodeHandles ret(children.size());
for (unsigned int i = 0; i < ret.size(); ++i) {
ret[i] = NodeHandle(children[i], shared_);
}
return ret;
}
RMF_NODE_CATCH();
}
NodeHandle TabNode::create_group( std::string name ) {
return NodeHandle( new WindowNode( shared_from_this(), name ) );
}
NodeHandle NodeHandle::add_child(std::string name, NodeType t) const {
try {
return NodeHandle(shared_->add_child(node_, name, t), shared_);
}
RMF_NODE_CATCH();
}
NodeHandle FileHandle::get_node_from_id(NodeID id) const {
return NodeHandle(id.get_index(), get_shared_data());
}
NodeHandle Node::create_tree_object( std::string name )
{
return NodeHandle( new TreeNode( shared_from_this(), path, name ) );
}
NodeHandle make_node() { return NodeHandle( new Node() ); }
simparm::NodeHandle Node::create_tree_root() { return NodeHandle( new NoOpNode( backend_node ) ); }
bool TFFController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle& n)
{
node_ = n;
// get name of root and tip from the parameter server
std::string root_name, tip_name;
if (!node_.getParam("root_name", root_name)){
ROS_ERROR("TFFController: No root name found on parameter server");
return false;
}
if (!node_.getParam("tip_name", tip_name)){
ROS_ERROR("TFFController: No tip name found on parameter server");
return false;
}
// test if we got robot pointer
assert(robot_state);
robot_state_ = robot_state;
// create robot chain from root to tip
if (!chain_.init(robot_state, root_name, tip_name))
return false;
chain_.toKDL(kdl_chain_);
// create solvers
jnt_to_twist_solver_.reset(new ChainFkSolverVel_recursive(kdl_chain_));
jnt_posvel_.resize(kdl_chain_.getNrOfJoints());
jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
jnt_pos_.resize(kdl_chain_.getNrOfJoints());
jnt_eff_.resize(kdl_chain_.getNrOfJoints());
jacobian_.resize(kdl_chain_.getNrOfJoints());
// twist to wrench
double trans, rot;
node_.param("twist_to_wrench_trans", trans, 0.0);
for (unsigned int i=0; i<3; i++)
twist_to_wrench_[i] = trans;
node_.param("twist_to_wrench_rot", rot, 0.0);
for (unsigned int i=3; i<6; i++)
twist_to_wrench_[i] = rot;
// get pid controllers
control_toolbox::Pid pid_controller;
if (!pid_controller.init(NodeHandle(node_, "vel_trans"))) return false;
for (unsigned int i=0; i<3; i++)
vel_pid_controller_.push_back(pid_controller);
if (!pid_controller.init(NodeHandle(node_, "vel_rot"))) return false;
for (unsigned int i=0; i<3; i++)
vel_pid_controller_.push_back(pid_controller);
if (!pid_controller.init(NodeHandle(node_, "pos_trans"))) return false;
for (unsigned int i=0; i<3; i++)
pos_pid_controller_.push_back(pid_controller);
if (!pid_controller.init(NodeHandle(node_, "pos_rot"))) return false;
for (unsigned int i=0; i<3; i++)
pos_pid_controller_.push_back(pid_controller);
// subscribe to tff commands
sub_command_ = node_.subscribe<tff_controller::TaskFrameFormalism>("command", 1,
&TFFController::command, this);
// realtime publisher for control state
state_position_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node_, "state/position", 1));
return true;
}