bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
int main(int argc, char** argv)
{
ROS_INFO("Started fullbody_teleop.");
ros::init(argc, argv, "fullbody_teleop");
ros::NodeHandle nh;
std::string robotDescription;
if (!nh.getParam("/robot_description", robotDescription))
{
ROS_FATAL("Parameter for robot description not provided");
}
huboModel.initString(robotDescription);
//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );
for (auto jointPair : huboModel.joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointPair.second;
if (joint->name[1] == 'F') { continue; }
if (joint->name== "TSY") { continue; }
makeJointMarker(joint->name);
}
std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;
ros::Duration(0.1).sleep();
for (int i = 0; i < HUBO_JOINT_COUNT; i++)
{
if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
{
planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
}
for (int side = 0; side < 2; side++)
for (int i = 1; i <= 3; i++)
for (int j = 1; j <= 3; j++)
{
std::string sideStr = (side == 0) ? "R" : "L";
planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
makeSaveButton();
gIntServer->applyChanges();
gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);
gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
gTimer.start();
ros::spin();
gIntServer.reset();
return 0;
}