void RobotStateDisplay::changedRobotStateTopic()
{
robot_state_subscriber_.shutdown();
robot_state_subscriber_ = root_nh_.subscribe(robot_state_topic_property_->getStdString(), 10, &RobotStateDisplay::newRobotStateCallback, this);
robot_->clear();
loadRobotModel();
}
bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
const std::string& group_name,
const std::string& base_name,
const std::string& tip_name,
double search_discretization)
{
setValues(robot_description, group_name, base_name, tip_name,search_discretization);
urdf::Model robot_model;
std::string xml_string;
ros::NodeHandle private_handle("~/"+group_name);
dimension_ = 7;
while(!loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
{
ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
ros::Duration(0.5).sleep();
}
ROS_DEBUG("Loading KDL Tree");
if(!getKDLChain(xml_string,base_frame_,tip_frame_,kdl_chain_))
{
active_ = false;
ROS_ERROR("Could not load kdl tree");
}
ROS_DEBUG("Advertising services");
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
private_handle.param<int>("free_angle",free_angle_,2);
pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model, base_frame_,tip_frame_, search_discretization_,free_angle_));
if(!pr2_arm_ik_solver_->active_)
{
ROS_ERROR("Could not load ik");
active_ = false;
}
else
{
pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
fk_solver_info_.joint_names = ik_solver_info_.joint_names;
for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
}
for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
}
for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
}
ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
active_ = true;
}
pr2_arm_ik_solver_->setFreeAngle(2);
return active_;
}
void PlanningSceneDisplay::reset()
{
planning_scene_render_.reset();
planning_scene_robot_->clear();
loadRobotModel();
Display::reset();
planning_scene_robot_->setVisible(scene_robot_enabled_property_->getBool());
}
void PlanningSceneDisplay::onEnable()
{
Display::onEnable();
loadRobotModel();
planning_scene_robot_->setVisible(scene_robot_enabled_property_->getBool());
if (planning_scene_render_)
planning_scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
calculateOffsetPosition();
}
void RobotStateDisplay::onEnable()
{
Display::onEnable();
loadRobotModel();
if (robot_)
{
changedEnableVisualVisible();
changedEnableCollisionVisible();
robot_->setVisible(true);
}
calculateOffsetPosition();
}
void RobotStateDisplay::reset()
{
robot_->clear();
rdf_loader_.reset();
loadRobotModel();
Display::reset();
changedEnableVisualVisible();
changedEnableCollisionVisible();
robot_->setVisible(true);
}
bool GaffaArmKinematicsPlugin::initialize(std::string name)
{
urdf::Model robot_model;
std::string tip_name, xml_string;
ros::NodeHandle private_handle("~/"+name);
dimension_ = 7;
while(!loadRobotModel(private_handle,robot_model,root_name_,tip_name,xml_string) && private_handle.ok())
{
ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
ros::Duration(0.5).sleep();
}
ROS_INFO("Loading KDL Tree");
if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
{
active_ = false;
ROS_ERROR("Could not load kdl tree");
}
ROS_INFO("Advertising services");
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
private_handle.param<int>("free_angle",free_angle_,2);
private_handle.param<double>("search_discretization",search_discretization_,0.01);
gaffa_arm_ik_solver_.reset(new gaffa_arm_kinematics::GaffaArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
if(!gaffa_arm_ik_solver_->active_)
{
ROS_ERROR("Could not load ik");
active_ = false;
}
else
{
gaffa_arm_ik_solver_->getSolverInfo(ik_solver_info_);
gaffa_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
fk_solver_info_.joint_names = ik_solver_info_.joint_names;
for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
{
ROS_INFO("GaffaKinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
}
for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
{
ROS_INFO("GaffaKinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
}
for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
{
ROS_INFO("GaffaKinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
}
ROS_INFO("GaffaKinematics::active");
active_ = true;
}
return active_;
}
PR2ArmKinematics::PR2ArmKinematics(bool create_tf_listener): node_handle_("~"),dimension_(7)
{
urdf::Model robot_model;
std::string tip_name, xml_string;
while(!loadRobotModel(node_handle_,robot_model, xml_string) && node_handle_.ok())
{
ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
ros::Duration(0.5).sleep();
}
if (!node_handle_.getParam("root_name", root_name_)){
ROS_FATAL("PR2IK: No root name found on parameter server");
exit(-1);
}
if (!node_handle_.getParam("tip_name", tip_name)){
ROS_FATAL("PR2IK: No tip name found on parameter server");
exit(-1);
}
ROS_DEBUG("Loading KDL Tree");
if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
{
active_ = false;
ROS_ERROR("Could not load kdl tree");
}
if(create_tf_listener) {
tf_ = new TransformListener();
} else {
tf_ = NULL;
}
ROS_DEBUG("Advertising services");
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
node_handle_.param<int>("free_angle",free_angle_,2);
node_handle_.param<double>("search_discretization",search_discretization_,0.01);
pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
if(!pr2_arm_ik_solver_->active_)
{
ROS_ERROR("Could not load ik");
active_ = false;
}
else
{
pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
fk_solver_info_.joint_names = ik_solver_info_.joint_names;
for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
}
for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
}
for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
}
ROS_DEBUG("PR2Kinematics::active");
active_ = true;
fk_service_ = node_handle_.advertiseService(FK_SERVICE,&PR2ArmKinematics::getPositionFK,this);
ik_service_ = node_handle_.advertiseService(IK_SERVICE,&PR2ArmKinematics::getPositionIK,this);
ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&PR2ArmKinematics::getIKSolverInfo,this);
fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&PR2ArmKinematics::getFKSolverInfo,this);
}
}
