/*! * \brief Publishes the state of the powercube_chain as ros messages. * * Published to "/joint_states" as "sensor_msgs/JointState" * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState" */ void publishState() { if (initialized_) { ROS_INFO("publish state"); pc_ctrl_->updateStates(); sensor_msgs::JointState joint_state_msg; joint_state_msg.header.stamp = ros::Time::now(); joint_state_msg.name = pc_params_->GetJointNames(); joint_state_msg.position = pc_ctrl_->getPositions(); joint_state_msg.velocity = pc_ctrl_->getVelocities(); pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg; controller_state_msg.header.stamp = joint_state_msg.header.stamp; controller_state_msg.joint_names = pc_params_->GetJointNames(); controller_state_msg.actual.positions = pc_ctrl_->getPositions(); controller_state_msg.actual.velocities = pc_ctrl_->getVelocities(); controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations(); topicPub_JointState_.publish(joint_state_msg); topicPub_ControllerState_.publish(controller_state_msg); last_publish_time_ = ros::Time::now(); } }
/*! * \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; }
/*! * \brief Publishes the state of the powercube_chain as ros messages. * * Published to "/joint_states" as "sensor_msgs/JointState" * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState" */ void publishState(bool update=true) { if (initialized_) { ROS_DEBUG("publish state"); if(update) {pc_ctrl_->updateStates();} sensor_msgs::JointState joint_state_msg; joint_state_msg.header.stamp = ros::Time::now(); joint_state_msg.name = pc_params_->GetJointNames(); joint_state_msg.position = pc_ctrl_->getPositions(); joint_state_msg.velocity = pc_ctrl_->getVelocities(); joint_state_msg.effort.resize(pc_params_->GetDOF()); pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg; controller_state_msg.header.stamp = joint_state_msg.header.stamp; controller_state_msg.joint_names = pc_params_->GetJointNames(); controller_state_msg.actual.positions = pc_ctrl_->getPositions(); controller_state_msg.actual.velocities = pc_ctrl_->getVelocities(); controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations(); std_msgs::String opmode_msg; opmode_msg.data = "velocity"; /// publishing joint and controller states on topic topicPub_JointState_.publish(joint_state_msg); topicPub_ControllerState_.publish(controller_state_msg); topicPub_OperationMode_.publish(opmode_msg); last_publish_time_ = joint_state_msg.header.stamp; } // check status of PowerCube chain if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; } // check status of PowerCube chain if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; } else { error_ = false; } // publishing diagnotic messages diagnostic_msgs::DiagnosticArray diagnostics; diagnostics.status.resize(1); // set data to diagnostics if(error_) { diagnostics.status[0].level = 2; diagnostics.status[0].name = n_.getNamespace();; diagnostics.status[0].message = pc_ctrl_->getErrorMessage(); } else { if (initialized_) { diagnostics.status[0].level = 0; diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain"; diagnostics.status[0].message = "powercubechain initialized and running"; } else { diagnostics.status[0].level = 1; diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain"; diagnostics.status[0].message = "powercubechain not initialized"; } } // publish diagnostic message topicPub_Diagnostic_.publish(diagnostics); }
/*! * \brief Executes the callback from the command_vel topic. * * Set the current velocity target. * \param msg JointVelocities */ void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg) { ROS_DEBUG("Received new velocity command"); if (!initialized_) { ROS_WARN("Skipping command: powercubes not initialized"); publishState(false); return; } if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { publishState(false); return; } PowerCubeCtrl::PC_CTRL_STATUS status; std::vector<std::string> errorMessages; pc_ctrl_->getStatus(status, errorMessages); /// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates) unsigned int DOF = pc_params_->GetDOF(); std::vector<std::string> jointNames = pc_params_->GetJointNames(); std::vector<double> cmd_vel(DOF); std::string unit = "rad"; /// check dimensions if (msg->velocities.size() != DOF) { ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension."); return; } /// parse velocities for (unsigned int i = 0; i < DOF; i++) { /// check joint name if (msg->velocities[i].joint_uri != jointNames[i]) { ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i); return; } /// check unit if (msg->velocities[i].unit != unit) { ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str()); return; } /// if all checks are successful, parse the velocity value for this joint ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str()); cmd_vel[i] = msg->velocities[i].value; } /// command velocities to powercubes if (!pc_ctrl_->MoveVel(cmd_vel)) { error_ = true; error_msg_ = pc_ctrl_->getErrorMessage(); ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str()); return; } ROS_DEBUG("Executed velocity command"); publishState(false); }
/*! * \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); }
/*! * \brief Gets parameters from the ROS parameter server and configures the powercube_chain. */ void getROSParameters() { /// get CanModule std::string CanModule; if (n_.hasParam("can_module")) { n_.getParam("can_module", CanModule); } else { ROS_ERROR("Parameter can_module not set, shutting down node..."); n_.shutdown(); } /// get CanDevice std::string CanDevice; if (n_.hasParam("can_device")) { n_.getParam("can_device", CanDevice); } else { ROS_ERROR("Parameter can_device not set, shutting down node..."); n_.shutdown(); } /// get CanBaudrate int CanBaudrate; if (n_.hasParam("can_baudrate")) { n_.getParam("can_baudrate", CanBaudrate); } else { ROS_ERROR("Parameter can_baudrate not set, shutting down node..."); n_.shutdown(); } /// get Modul IDs XmlRpc::XmlRpcValue ModulIDsXmlRpc; std::vector<int> ModulIDs; if (n_.hasParam("modul_ids")) { n_.getParam("modul_ids", ModulIDsXmlRpc); } else { ROS_ERROR("Parameter modul_ids not set, shutting down node..."); n_.shutdown(); } /// get force_use_movevel bool UseMoveVel; if (n_.hasParam("force_use_movevel")) { n_.getParam("force_use_movevel", UseMoveVel); ROS_INFO("Parameter force_use_movevel set, using moveVel"); } else { ROS_INFO("Parameter force_use_movevel not set, using moveStep"); UseMoveVel = false; } pc_params_->SetUseMoveVel(UseMoveVel); /// Resize and assign of values to the ModulIDs ModulIDs.resize(ModulIDsXmlRpc.size()); for (int i = 0; i < ModulIDsXmlRpc.size(); i++) { ModulIDs[i] = (int)ModulIDsXmlRpc[i]; } /// Initialize parameters pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs); /// Get joint names XmlRpc::XmlRpcValue JointNamesXmlRpc; std::vector<std::string> JointNames; if (n_.hasParam("joint_names")) { n_.getParam("joint_names", JointNamesXmlRpc); } else { ROS_ERROR("Parameter joint_names not set, shutting down node..."); n_.shutdown(); } /// Resize and assign of values to the JointNames JointNames.resize(JointNamesXmlRpc.size()); for (int i = 0; i < JointNamesXmlRpc.size(); i++) { JointNames[i] = (std::string)JointNamesXmlRpc[i]; } /// Check dimension with with DOF if ((int)JointNames.size() != pc_params_->GetDOF()) { ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node..."); n_.shutdown(); } pc_params_->SetJointNames(JointNames); /// Get max accelerations XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc; std::vector<double> MaxAccelerations; if (n_.hasParam("max_accelerations")) { n_.getParam("max_accelerations", MaxAccelerationsXmlRpc); } else { ROS_ERROR("Parameter max_accelerations not set, shutting down node..."); n_.shutdown(); } /// Resize and assign of values to the MaxAccelerations MaxAccelerations.resize(MaxAccelerationsXmlRpc.size()); for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++) { MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i]; } /// Check dimension with with DOF if ((int)MaxAccelerations.size() != pc_params_->GetDOF()) { ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node..."); n_.shutdown(); } pc_params_->SetMaxAcc(MaxAccelerations); /// Get horizon double Horizon; if (n_.hasParam("horizon")) { n_.getParam("horizon", Horizon); } else { /// Horizon in sec Horizon = 0.05; ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon); } pc_ctrl_->setHorizon(Horizon); }
/*! * \brief Gets parameters from the ROS parameter server and configures the powercube_chain. */ void getROSParameters() { // get CanModule std::string CanModule; if (n_.hasParam("can_module")) { n_.getParam("can_module", CanModule); } else { ROS_ERROR("Parameter can_module not set, shutting down node..."); n_.shutdown(); } // get CanDevice std::string CanDevice; if (n_.hasParam("can_device")) { n_.getParam("can_device", CanDevice); } else { ROS_ERROR("Parameter can_device not set, shutting down node..."); n_.shutdown(); } // get CanBaudrate int CanBaudrate; if (n_.hasParam("can_baudrate")) { n_.getParam("can_baudrate", CanBaudrate); } else { ROS_ERROR("Parameter can_baudrate not set, shutting down node..."); n_.shutdown(); } // get modul ids XmlRpc::XmlRpcValue ModulIDsXmlRpc; std::vector<int> ModulIDs; if (n_.hasParam("modul_ids")) { n_.getParam("modul_ids", ModulIDsXmlRpc); } else { ROS_ERROR("Parameter modul_ids not set, shutting down node..."); n_.shutdown(); } ModulIDs.resize(ModulIDsXmlRpc.size()); for (int i = 0; i < ModulIDsXmlRpc.size(); i++) { ModulIDs[i] = (int)ModulIDsXmlRpc[i]; } // init parameters pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs); // get joint names XmlRpc::XmlRpcValue JointNamesXmlRpc; std::vector<std::string> JointNames; if (n_.hasParam("joint_names")) { n_.getParam("joint_names", JointNamesXmlRpc); } else { ROS_ERROR("Parameter joint_names not set, shutting down node..."); n_.shutdown(); } JointNames.resize(JointNamesXmlRpc.size()); for (int i = 0; i < JointNamesXmlRpc.size(); i++) { JointNames[i] = (std::string)JointNamesXmlRpc[i]; } // check dimension with with DOF if ((int)JointNames.size() != pc_params_->GetDOF()) { ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node..."); n_.shutdown(); } pc_params_->SetJointNames(JointNames); // get max accelerations XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc; std::vector<double> MaxAccelerations; if (n_.hasParam("max_accelerations")) { n_.getParam("max_accelerations", MaxAccelerationsXmlRpc); } else { ROS_ERROR("Parameter max_accelerations not set, shutting down node..."); n_.shutdown(); } MaxAccelerations.resize(MaxAccelerationsXmlRpc.size()); for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++) { MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i]; } // check dimension with with DOF if ((int)MaxAccelerations.size() != pc_params_->GetDOF()) { ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node..."); n_.shutdown(); } pc_params_->SetMaxAcc(MaxAccelerations); // get horizon double Horizon; if (n_.hasParam("horizon")) { n_.getParam("horizon", Horizon); } else { Horizon = 0.025; //Hz ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon); } pc_ctrl_->setHorizon(Horizon); }