std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{
std::string full_param_name;
std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points;
if (nh.searchParam("footprint", full_param_name))
{
XmlRpc::XmlRpcValue footprint_xmlrpc;
nh.getParam(full_param_name, footprint_xmlrpc);
if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
{
if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
{
writeFootprintToParam(nh, points);
}
}
else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
writeFootprintToParam(nh, points);
}
}
else if (nh.searchParam("robot_radius", full_radius_param_name))
{
double robot_radius;
nh.param(full_radius_param_name, robot_radius, 1.234);
points = makeFootprintFromRadius(robot_radius);
nh.setParam("robot_radius", robot_radius);
}
// Else neither param was found anywhere this knows about, so
// defaults will come from dynamic_reconfigure stuff, set in
// cfg/Costmap2D.cfg and read in this file in reconfigureCB().
return points;
}
bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
{
std::string urdf_xml,full_urdf_xml;
node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
node_handle.searchParam(urdf_xml,full_urdf_xml);
TiXmlDocument xml;
ROS_DEBUG("Reading xml file from parameter server\n");
std::string result;
if (node_handle.getParam(full_urdf_xml, result))
xml.Parse(result.c_str());
else
{
ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
return false;
}
xml_string = result;
TiXmlElement *root_element = xml.RootElement();
TiXmlElement *root = xml.FirstChildElement("robot");
if (!root || !root_element)
{
ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
exit(1);
}
robot_model.initXml(root);
if (!node_handle.getParam("root_name", root_name)){
ROS_FATAL("No root name found on parameter server");
return false;
}
if (!node_handle.getParam("tip_name", tip_name)){
ROS_FATAL("No tip name found on parameter server");
return false;
}
return true;
}
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");
}
// Get the URDF XML from the parameter server
std::string getURDF(ros::NodeHandle &model_nh_, std::string param_name)
{
std::string urdf_string;
std::string robot_description = "/robot_description";
// search and wait for robot_description on param server
while (urdf_string.empty())
{
std::string search_param_name;
if (model_nh_.searchParam(param_name, search_param_name))
{
ROS_INFO_ONCE_NAMED("LWRHWFRIL", "LWRHWFRIL node is waiting for model"
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
model_nh_.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_ONCE_NAMED("LWRHWFRIL", "LWRHWFRIL node is waiting for model"
" URDF in parameter [%s] on the ROS param server.", robot_description.c_str());
model_nh_.getParam(param_name, urdf_string);
}
usleep(100000);
}
ROS_DEBUG_STREAM_NAMED("LWRHWFRIL", "Recieved urdf from param server, parsing...");
return urdf_string;
}
void CollisionPluginLoader::setupScene(
ros::NodeHandle& nh,
const planning_scene::PlanningScenePtr& scene)
{
if (!scene)
return;
std::string param_name;
std::string collision_detector_name;
if (nh.searchParam("collision_detector", param_name))
{
nh.getParam(param_name, collision_detector_name);
}
else if (nh.hasParam("/move_group/collision_detector"))
{
// Check for existence in move_group namespace
// mainly for rviz plugins to get same collision detector.
nh.getParam("/move_group/collision_detector", collision_detector_name);
}
else
{
return;
}
if (collision_detector_name == "")
{
// This is not a valid name for a collision detector plugin
return;
}
activate(collision_detector_name, scene, true);
ROS_INFO_STREAM("Using collision detector:" << scene->getActiveCollisionDetectorName().c_str());
}
bool ExcavaROBArmKinematics::init() {
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
nh_.param("urdf_xml", urdf_xml, std::string("robot_description"));
nh_.searchParam(urdf_xml, full_urdf_xml);
ROS_DEBUG("ExcavaROB Kinematics: Reading xml file from parameter server");
std::string result;
if (!nh_.getParam(full_urdf_xml, result)) {
ROS_FATAL("ExcavaROB Kinematics: Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root, Wrist and Tip From Parameter Service
if (!nh_private_.getParam("root_name", root_name_)) {
ROS_FATAL("ExcavaROB Kinematics: No root_name found on parameter server");
return false;
}
if (!nh_private_.getParam("tip_name", tip_name_)) {
ROS_FATAL("ExcavaROB Kinematics: No tip_name found on parameter server");
return false;
}
if (!nh_private_.getParam("max_iterations", max_iterations_)) {
ROS_FATAL("ExcavaROB Kinematics: No max_iterations found on parameter server");
return false;
}
if (!nh_private_.getParam("eps", eps_)) {
ROS_FATAL("ExcavaROB Kinematics: No eps found on parameter server");
return false;
}
if (!nh_private_.getParam("ik_gain", ik_gain_)) {
ROS_FATAL("ExcavaROB Kinematics: No ik_gain found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int maxIterations;
double epsilon;
nh_private_.param("maxIterations", maxIterations, 1000);
nh_private_.param("epsilon", epsilon, 1e-2);
// Build Solvers
fk_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
jnt_to_jac_solver_ = new KDL::ChainJntToJacSolver(arm_chain_);
jnt_to_pose_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
ROS_INFO("ExcavaROB Kinematics: Advertising services");
ik_service_ = nh_private_.advertiseService(IK_SERVICE, &ExcavaROBArmKinematics::getPositionIK, this);
fk_service_ = nh_private_.advertiseService(FK_SERVICE, &ExcavaROBArmKinematics::getPositionFK, this);
kin_solver_info_service_ = nh_private_.advertiseService(KIN_INFO_SERVICE, &ExcavaROBArmKinematics::getKinSolverInfo, this);
return true;
}
bool Kinematics::init() {
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
nh.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!nh.getParam(full_urdf_xml, result)) {
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
std::string kinematics_service_name;
if (!nh_private.getParam("kinematics_service_name", kinematics_service_name)) {
ROS_FATAL("GenericIK: kinematics service name not found on parameter server");
return false;
}
// Get Root and Tip From Parameter Service
if (!nh.getParam(kinematics_service_name+"/manipulator/root_name", root_name)) {
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!nh.getParam(kinematics_service_name+"/manipulator/tip_name", tip_name)) {
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int maxIterations;
double epsilon;
nh_private.param("maxIterations", maxIterations, 1000);
nh_private.param("epsilon", epsilon, 1e-2); // .01
// Build Solvers
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, maxIterations, epsilon);
ROS_INFO("Advertising services");
fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);
// ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::searchPositionIK,this);
ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);
return true;
}
/*!
* \brief Constructor for PowercubeChainNode class.
*
* \param name Name for the actionlib server.
*/
PowercubeChainNode(std::string name):
as_(n_, name, boost::bind(&PowercubeChainNode::executeCB, this, _1)),
action_name_(name)
{
sem_can_available = false;
can_sem = SEM_FAILED;
isInitialized_ = false;
traj_point_nr_ = 0;
finished_ = false;
#ifndef SIMU
PCube_ = new PowerCubeCtrl();
#else
PCube_ = new simulatedArm();
#endif
PCubeParams_ = new PowerCubeCtrlParams();
// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
// implementation of topics to subscribe
topicSub_DirectCommand_ = n_.subscribe("command", 1, &PowercubeChainNode::topicCallback_DirectCommand, this);
// implementation of service servers
srvServer_Init_ = n_.advertiseService("init", &PowercubeChainNode::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("stop", &PowercubeChainNode::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("recover", &PowercubeChainNode::srvCallback_Recover, this);
srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowercubeChainNode::srvCallback_SetOperationMode, this);
// implementation of service clients
//--
// diagnostics
updater_.setHardwareID(ros::this_node::getName());
updater_.add("initialization", this, &PowercubeChainNode::diag_init);
// read parameters from parameter server
n_.getParam("CanModule", CanModule_);
n_.getParam("CanDevice", CanDevice_);
n_.getParam("CanBaudrate", CanBaudrate_);
ROS_INFO("CanModule=%s, CanDevice=%d, CanBaudrate=%d",CanModule_.c_str(),CanDevice_,CanBaudrate_);
// get ModIds from parameter server
if (n_.hasParam("ModIds"))
{
n_.getParam("ModIds", ModIds_param_);
}
else
{
ROS_ERROR("Parameter ModIds not set");
}
ModIds_.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
ModIds_[i] = (int)ModIds_param_[i];
}
std::cout << "ModIds = " << ModIds_param_ << std::endl;
// get JointNames from parameter server
ROS_INFO("getting JointNames from parameter server");
if (n_.hasParam("JointNames"))
{
n_.getParam("JointNames", JointNames_param_);
}
else
{
ROS_ERROR("Parameter JointNames not set");
}
JointNames_.resize(JointNames_param_.size());
for (int i = 0; i<JointNames_param_.size(); i++ )
{
JointNames_[i] = (std::string)JointNames_param_[i];
}
std::cout << "JointNames = " << JointNames_param_ << std::endl;
PCubeParams_->Init(CanModule_, CanDevice_, CanBaudrate_, ModIds_);
// get MaxAcc from parameter server
ROS_INFO("getting MaxAcc from parameter server");
if (n_.hasParam("MaxAcc"))
{
n_.getParam("MaxAcc", MaxAcc_param_);
}
else
{
ROS_ERROR("Parameter MaxAcc not set");
}
MaxAcc_.resize(MaxAcc_param_.size());
for (int i = 0; i<MaxAcc_param_.size(); i++ )
{
MaxAcc_[i] = (double)MaxAcc_param_[i];
}
PCubeParams_->SetMaxAcc(MaxAcc_);
std::cout << "MaxAcc = " << MaxAcc_param_ << std::endl;
// load parameter server string for robot/model
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name,full_param_name);
n_.getParam(full_param_name.c_str(),xml_string);
ROS_INFO("full_param_name=%s",full_param_name.c_str());
if (xml_string.size()==0)
{
ROS_ERROR("Unable to load robot model from param server robot_description\n");
exit(2);
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// extract limits and velocities from urdf model
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
exit(2);
}
ROS_INFO("Successfully parsed urdf file");
/** @todo: check if yaml parameter file fits to urdf model */
// get MaxVel out of urdf model
std::vector<double> MaxVel;
MaxVel.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
MaxVel[i] = model.getJoint(JointNames_[i].c_str())->limits->velocity;
//std::cout << "MaxVel[" << JointNames_[i].c_str() << "] = " << MaxVel[i] << std::endl;
}
PCubeParams_->SetMaxVel(MaxVel);
// get LowerLimits out of urdf model
std::vector<double> LowerLimits;
LowerLimits.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
LowerLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->lower;
std::cout << "LowerLimits[" << JointNames_[i].c_str() << "] = " << LowerLimits[i] << std::endl;
}
PCubeParams_->SetLowerLimits(LowerLimits);
// get UpperLimits out of urdf model
std::vector<double> UpperLimits;
UpperLimits.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
UpperLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->upper;
std::cout << "UpperLimits[" << JointNames_[i].c_str() << "] = " << UpperLimits[i] << std::endl;
}
PCubeParams_->SetUpperLimits(UpperLimits);
// get UpperLimits out of urdf model
std::vector<double> Offsets;
Offsets.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
Offsets[i] = model.getJoint(JointNames_[i].c_str())->calibration->rising.get()[0];
std::cout << "Offset[" << JointNames_[i].c_str() << "] = " << Offsets[i] << std::endl;
}
PCubeParams_->SetAngleOffsets(Offsets);
cmd_vel_.resize(ModIds_param_.size());
newvel_ = false;
can_sem = sem_open("/semcan", O_CREAT, S_IRWXU | S_IRWXG,1);
if (can_sem != SEM_FAILED)
sem_can_available = true;
}
// Constructor
NodeClass(std::string name):
as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
action_name_(name)
{
isInitialized_ = false;
ActualPos_=0.0;
ActualVel_=0.0;
CamAxis_ = new ElmoCtrl();
CamAxisParams_ = new ElmoCtrlParams();
// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
// implementation of topics to subscribe
// implementation of service servers
srvServer_Init_ = n_.advertiseService("Init", &NodeClass::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("Stop", &NodeClass::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("Recover", &NodeClass::srvCallback_Recover, this);
// implementation of service clients
//--
// read parameters from parameter server
n_.getParam("CanDevice", CanDevice_);
n_.getParam("CanBaudrate", CanBaudrate_);
n_.getParam("HomingDir", HomingDir_);
n_.getParam("ModId",ModID_);
n_.getParam("JointName",JointName_);
n_.getParam("CanIniFile",CanIniFile_);
n_.getParam("OperationMode",operationMode_);
//n_.param<double>("MaxVel", MaxVel_, 2.0); -> from urdf
ROS_INFO("CanDevice=%s, CanBaudrate=%d,ModID=%d",CanDevice_.c_str(),CanBaudrate_,ModID_);
// load parameter server string for robot/model
std::string param_name = "/robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name,full_param_name);
n_.getParam(full_param_name.c_str(),xml_string);
ROS_INFO("full_param_name=%s",full_param_name.c_str());
if (xml_string.size()==0)
{
ROS_ERROR("Unable to load robot model from param server robot_description\n");
exit(2);
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// extract limits and velocitys from urdf model
// The urdf model can be found in torso_cob3-1.urdf -> joint_head_eyes
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
exit(2);
}
ROS_INFO("Successfully parsed urdf file");
// get LowerLimits out of urdf model
LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetLowerLimit(LowerLimit_);
// get UpperLimits out of urdf model
UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetUpperLimit(UpperLimit_);
// get Offset out of urdf model
Offset_ = model.getJoint(JointName_.c_str())->calibration->reference_position;
//std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
CamAxisParams_->SetAngleOffset(Offset_);
// get velocitiy out of urdf model
MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
ROS_INFO("Successfully read limits from urdf");
//initializing and homing of camera_axis
CamAxisParams_->SetCanIniFile(CanIniFile_);
CamAxisParams_->SetHomingDir(HomingDir_);
CamAxisParams_->SetMaxVel(MaxVel_);
CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
}
bool ExcavaROBOdometry::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
{
node_ = node;
std::string tf_prefix_param;
node.searchParam("tf_prefix", tf_prefix_param);
node.getParam(tf_prefix_param, tf_prefix_);
node.param("odometer/initial_distance", odometer_initial_distance_, 0.0);
node.param("odometer/initial_angle", odometer_initial_angle_, 0.0);
node.param("odom/initial_x", odom_.x, 0.0);
node.param("odom/initial_y", odom_.y, 0.0);
node.param("odom/initial_yaw", odom_.z, 0.0);
node.param("publish_tf", publish_tf_, true);
node.param<std::string> ("odom_frame", odom_frame_, "odom");
node.param<std::string> ("base_footprint_frame", base_footprint_frame_, "base_footprint");
node.param<std::string> ("base_link_frame", base_link_frame_, "drivetrain_link");
node.param<double> ("track_distance", track_distance_, 2.2542373);
node.param<double> ("x_stddev", sigma_x_, 0.002);
node.param<double> ("y_stddev", sigma_y_, 0.002);
node.param<double> ("rotation_stddev", sigma_theta_, 0.017);
node.param<double> ("cov_xy", cov_xy_, 0.0);
node.param<double> ("cov_xrotation", cov_x_theta_, 0.0);
node.param<double> ("cov_yrotation", cov_y_theta_, 0.0);
node.param<bool> ("verbose", verbose_, false);
node.param("odom_publish_rate", odom_publish_rate_, 30.0);
node.param("odometer_publish_rate", odometer_publish_rate_, 1.0);
node.param("odometry_state_publish_rate", odometry_state_publish_rate_, 1.0);
if (odom_publish_rate_ <= 0.0) {
expected_publish_time_ = 0.0;
publish_odom_ = false;
}
else {
expected_publish_time_ = 1.0 / odom_publish_rate_;
publish_odom_ = true;
}
if(odometer_publish_rate_ <= 0.0) {
expected_odometer_publish_time_ = 0.0;
publish_odometer_ = false;
}
else {
expected_odometer_publish_time_ = 1.0 / odometer_publish_rate_;
publish_odometer_ = true;
}
if(odometry_state_publish_rate_ <= 0.0) {
expected_state_publish_time_ = 0.0;
publish_state_odometry_ = false;
}
else {
expected_state_publish_time_ = 1.0 / odometry_state_publish_rate_;
publish_state_odometry_ = true;
}
if (!base_kin_.init(robot_state, node_))
return false;
for (int i = 0; i < base_kin_.right_num_wheels_; i++) {
if (!base_kin_.right_wheel_[i].joint_->calibrated_) {
ROS_ERROR("Base odometry could not start because the wheels were not calibrated. Relaunch the odometry after you see the wheel calibration finish.");
return false;
}
}
for (int i = 0; i < base_kin_.left_num_wheels_; i++) {
if (!base_kin_.left_wheel_[i].joint_->calibrated_) {
ROS_ERROR("Base odometry could not start because the wheels were not calibrated. Relaunch the odometry after you see the wheel calibration finish.");
return false;
}
}
if (verbose_) {
debug_publisher_.reset(new realtime_tools::RealtimePublisher<excavaROB_mechanism_controllers::DebugInfo> (node_,"debug", 1));
debug_publisher_->msg_.timing.resize(3);
}
odometry_state_publisher_.reset(new realtime_tools::RealtimePublisher<excavaROB_mechanism_controllers::BaseOdometryState> (node_,"state", 1));
odometer_publisher_.reset(new realtime_tools::RealtimePublisher<excavaROB_mechanism_controllers::Odometer> (node_,"odometer", 1));
odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry> (node_,odom_frame_, 1));
transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage> (node_,"/tf", 1));
transform_publisher_->msg_.transforms.resize(1);
odometry_state_publisher_->msg_.right_wheel_link_names.resize(base_kin_.right_num_wheels_);
odometry_state_publisher_->msg_.left_wheel_link_names.resize(base_kin_.left_num_wheels_);
return true;
}
// Constructor
NodeClass(std::string name):
as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
action_name_(name)
{
n_ = ros::NodeHandle("~");
isInitialized_ = false;
ActualPos_=0.0;
ActualVel_=0.0;
CamAxis_ = new ElmoCtrl();
CamAxisParams_ = new ElmoCtrlParams();
// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
// implementation of topics to subscribe
// implementation of service servers
srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
// implementation of service clients
//--
// read parameters from parameter server
if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");
n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
n_.param<int>("CanBaudrate", CanBaudrate_, 500);
n_.param<int>("HomingDir", HomingDir_, 1);
n_.param<int>("HomingDigIn", HomingDigIn_, 11);
n_.param<int>("ModId",ModID_, 17);
n_.param<std::string>("JointName",JointName_, "head_axis_joint");
n_.param<std::string>("CanIniFile",CanIniFile_, "/");
n_.param<std::string>("operation_mode",operationMode_, "position");
n_.param<int>("MotorDirection",MotorDirection_, 1);
n_.param<double>("GearRatio",GearRatio_, 62.5);
n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
// load parameter server string for robot/model
std::string param_name = "/robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name,full_param_name);
n_.getParam(full_param_name.c_str(),xml_string);
ROS_INFO("full_param_name=%s",full_param_name.c_str());
if (xml_string.size()==0)
{
ROS_ERROR("Unable to load robot model from param server robot_description\n");
exit(2);
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// extract limits and velocitys from urdf model
// The urdf model can be found in torso_cob3-1.urdf -> joint_head_eyes
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
exit(2);
}
ROS_INFO("Successfully parsed urdf file");
// get LowerLimits out of urdf model
LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetLowerLimit(LowerLimit_);
// get UpperLimits out of urdf model
UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
//std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
CamAxisParams_->SetUpperLimit(UpperLimit_);
// get Offset out of urdf model
Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
//std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
CamAxisParams_->SetAngleOffset(Offset_);
// get velocitiy out of urdf model
MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
ROS_INFO("Successfully read limits from urdf");
//initializing and homing of camera_axis
CamAxisParams_->SetCanIniFile(CanIniFile_);
CamAxisParams_->SetHomingDir(HomingDir_);
CamAxisParams_->SetHomingDigIn(HomingDigIn_);
CamAxisParams_->SetMaxVel(MaxVel_);
CamAxisParams_->SetGearRatio(GearRatio_);
CamAxisParams_->SetMotorDirection(MotorDirection_);
CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
}
/*!
* \brief Gets parameters from the robot_description and configures the powercube_chain.
*/
void getRobotDescriptionParameters()
{
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> JointNames = pc_params_->GetJointNames();
/// Get robot_description from ROS parameter server
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name, full_param_name);
if (n_.hasParam(full_param_name))
{
n_.getParam(full_param_name.c_str(), xml_string);
}
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
n_.shutdown();
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
n_.shutdown();
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
/// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n_.shutdown();
}
ROS_DEBUG("Successfully parsed urdf file");
/// Get max velocities out of urdf model
std::vector<double> MaxVelocities(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
}
/// Get lower limits out of urdf model
std::vector<double> LowerLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
}
// Get upper limits out of urdf model
std::vector<double> UpperLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
}
/// Get offsets out of urdf model
std::vector<double> Offsets(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
}
/// Set parameters
pc_params_->SetMaxVel(MaxVelocities);
pc_params_->SetLowerLimits(LowerLimits);
pc_params_->SetUpperLimits(UpperLimits);
pc_params_->SetOffsets(Offsets);
}
// load robot footprint from costmap_2d_ros to keep same footprint format
std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
double padding;
std::string padding_param, footprint_param;
if(!node.searchParam("footprint_padding", padding_param))
padding = 0.01;
else
node.param(padding_param, padding, 0.01);
//grab the footprint from the parameter server if possible
XmlRpc::XmlRpcValue footprint_list;
std::string footprint_string;
std::vector<std::string> footstring_list;
if(node.searchParam("footprint", footprint_param)){
node.getParam(footprint_param, footprint_list);
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
footprint_string = std::string(footprint_list);
//if there's just an empty footprint up there, return
if(footprint_string == "[]" || footprint_string == "")
return footprint;
boost::erase_all(footprint_string, " ");
boost::char_separator<char> sep("[]");
boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
}
//make sure we have a list of lists
if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < footprint_list.size(); ++i){
//make sure we have a list of lists of size 2
XmlRpc::XmlRpcValue point = footprint_list[i];
if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
}
//make sure that the value we're looking at is either a double or an int
if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
pt.x += sign(pt.x) * padding;
//make sure that the value we're looking at is either a double or an int
if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
pt.y += sign(pt.y) * padding;
footprint.push_back(pt);
node.deleteParam(footprint_param);
std::ostringstream oss;
bool first = true;
BOOST_FOREACH(geometry_msgs::Point p, footprint) {
if(first) {
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else {
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
node.setParam(footprint_param, oss.str().c_str());
node.setParam("footprint", oss.str().c_str());
}
}
else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
