int CSnapshot::GetItemSize(int Index) { if(Index == m_NumItems-1) return (m_DataSize - Offsets()[Index]) - sizeof(CSnapshotItem); return (Offsets()[Index+1] - Offsets()[Index]) - sizeof(CSnapshotItem); }
CSnapshotItem *CSnapshot::GetItem(int Index) { return (CSnapshotItem *)(DataStart() + Offsets()[Index]); }
/*! * \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); }