/*!
* \brief Executes the service callback for set_default_vel.
*
* Sets the default velocity.
* \param req Service request
* \param res Service response
*/
bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
cob_srvs::SetDefaultVel::Response &res )
{
ROS_INFO("Set default velocity to [%f]", req.default_vel);
MaxVel_ = req.default_vel;
CamAxisParams_->SetMaxVel(MaxVel_);
res.success.data = true; // 0 = true, else = false
return 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_);
}
// 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_);
}
