// 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_); }