void TiledMapLoader::loadNamedFactories(cocos2d::TMXTiledMap& map) { for (auto& arrayElement : map.getObjectGroups()) { auto objectGroup = dynamic_cast<TMXObjectGroup*>(arrayElement); if (!objectGroup) { continue; } for (auto& value : objectGroup->getObjects()) { auto valueMap = value.asValueMap(); if (valueMap.empty()) continue; if (!valueMap.count("name")) { continue; } auto name = valueMap["name"].asString(); if (!nameFactories.count(name)) { continue; } Configuration config{valueMap, objectGroup->getGroupName(), map, box2dContainer}; for (auto& callback : nameFactories.at(name)) { callback(config); } } } }
void my_list(int data_conn_sock ,char *filepath) { char mymode[11]= "----------"; struct stat buf; struct dirent *p = NULL; struct passwd *passwd; char uname[50]; char gname[50]; char mtime[50]; DIR *mydir = NULL; char buffer[100]; chdir(filepath); mydir = opendir(filepath); while( (p = readdir(mydir)) != NULL ) { memset( &buf, 0, sizeof(buf)); stat(p->d_name,&buf); getFileMode(buf.st_mode, mymode); getUserName(buf.st_uid, uname); getGroupName(buf.st_gid, gname); getTime(buf.st_mtime, mtime); sprintf(buffer,"%s %d %s %s %10d %s %s\r\n", mymode, buf.st_nlink, uname, gname, buf.st_size, mtime, p->d_name); if (data_conn_sock != 0) { write_loop(data_conn_sock, buffer, strlen(buffer)); } } closedir(mydir); }
gkSkeletonResource* gkSkeletonResource::clone() { GK_ASSERT(m_creator); gkResourceName skelName(gkUtils::getUniqueName(getName()), getGroupName()); gkSkeletonResource* cl = m_creator->create<gkSkeletonResource>(skelName); cl->copyBones(*this); return cl; }
QString QtUMItem::getDisplayName() const { if ( isAGroupItem() ) { return getGroupName(); } else { return _displayName; } }
void QtUMItemManager::sortChildren( QTreeWidgetItem* parentItem) { int count = parentItem->childCount(); QtUMItemInfoList itemInfoList; int sortOrder = 0; bool isGroup = 0; QString userName = ""; QTreeWidgetItem* item = NULL; QtUMItem* pItem = NULL; QtUMGroup* pGroup = NULL; EnumPresenceState::PresenceState presence = EnumPresenceState::PresenceStateUnknown; for (int index = 0; index < count; ++index) { item = parentItem->child(index); pItem = getQtUMItemFromItem( item ); pGroup = getQtUMGroupFromItem( item ); //TODO: VOXOX CHANGE by Rolando - 2009.08.31 - to check it because here there is an error if ( !pItem )//VOXOX CHANGE by Rolando - 2009.08.31 - if item is a group { if(pGroup){ QString groupId = QString( item->text(0) ); userName = getGroupName(groupId);//VOXOX CHANGE by Rolando - 2009.09.03 presence = EnumPresenceState::PresenceStateUnknown; isGroup = true; } } else { userName = (pItem ? QString(pItem->getDisplayName() ) : "" ); presence = (pItem ? pItem->getPresenceState() : EnumPresenceState::PresenceStateUnknown); isGroup = false; } QtUMItemInfo info = QtUMItemInfo(item, parentItem, item->text(0), userName, index, isGroup); itemInfoList.append(info); //End VoxOx } qSort(itemInfoList); Q_FOREACH(QtUMItemInfo info, itemInfoList) { QTreeWidgetItem* item = info.getItem(); parentItem->takeChild ( parentItem->indexOfChild(item)); parentItem->insertChild( parentItem->childCount(), item); }
void gkOgreParticleResource::createParticle(const gkParticleSettingsProperties& props) { GK_ASSERT(m_psys == 0); try { Ogre::ParticleSystemManager& mgr = Ogre::ParticleSystemManager::getSingleton(); Ogre::ParticleSystem* psys = mgr.getTemplate(getName()); if (!psys) { psys = mgr.createTemplate(getName(), getGroupName()); m_isTemplateOwner = true; gkOgreParticleAffector* affector = static_cast<gkOgreParticleAffector*>(psys->addAffector(gkOgreParticleAffector::NAME)); affector->setCreator(this); gkOgreParticleEmitter* emitter = static_cast<gkOgreParticleEmitter*>(psys->addEmitter(gkOgreParticleEmitter::NAME)); emitter->setCreator(this); psys->setCullIndividually(false); psys->setDefaultDimensions(props.m_size, props.m_size); psys->setParticleQuota(props.m_amount); //emitter->setStartTime(props.m_start); //emitter->setTimeToLive(props.m_lifetime); //emitter->setDirection(props.m_velocity); //emitter->setParticleVelocity(props.m_velocity.length()); psys->setParameter("billboard_type", "oriented_self"); //emitter->setEmissionRate(props.m_velNormal); } m_particleProps = props; m_psys = psys; } catch (Ogre::Exception& e) { gkLogMessage("gkOgreParticleResource: " << e.getDescription()); } }
gkGameObject* gkScene::getObject(const gkHashedString& name) { UTsize pos = m_objects.find(name); if (pos != GK_NPOS) return m_objects.at(pos); else { gkGameObjectManager& mgr = gkGameObjectManager::getSingleton(); gkResourceName rname(name, getGroupName()); if (mgr.exists(rname)) { gkGameObject* obj = mgr.getByName<gkGameObject>(rname); if (obj && obj->getOwner() == this) return obj; } } return 0; }
void gkOgreParticleObject::createInstanceImpl() { gkParticleObject::createInstanceImpl(); GK_ASSERT(!m_psys); try { Ogre::SceneManager* manager = m_scene->getManager(); const gkString& pname = m_particleProps.m_settings; m_psys = manager->createParticleSystem(getName(), m_particleProps.m_settings); if (m_psys) { m_psys->setUserAny(Ogre::Any(this)); m_node->attachObject(m_psys); gkOgreParticleResource* resource = gkParticleManager::getSingleton().getByName<gkOgreParticleResource>( gkResourceName(pname, getGroupName())); if (resource && resource->isTemplateOwner()) { m_psys->setRenderer(gkOgreParticleRenderer::NAME); if (resource->getParticleProperties().m_render == gkParticleSettingsProperties::R_HALO) m_psys->setMaterialName(gkParticleManager::getSingleton().createOrRetrieveHaloMaterial(m_particleProps.m_material)); else m_psys->setMaterialName(m_particleProps.m_material); } //else create by .particle script } } catch (Ogre::Exception& e) { gkLogMessage("gkOgreParticleObject: " << e.getDescription()); } }
void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state, const std::vector<double> &consistency_limits, KDL::JntArray &jnt_array) const { std::vector<double> values, near; for(std::size_t i=0; i < dimension_; ++i) { near.push_back(seed_state(i)); } robot_state::JointStateGroup* joint_state_group = kinematic_state_->getJointStateGroup(getGroupName()); joint_state_group->setToRandomValuesNearBy(near, consistency_limits); joint_state_group->getVariableValues(values); for(std::size_t i=0; i < dimension_; ++i) { jnt_array(i) = values[i]; } }
void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array) const { std::vector<double> jnt_array_vector(dimension_,0.0); robot_state::JointStateGroup* joint_state_group = kinematic_state_->getJointStateGroup(getGroupName()); joint_state_group->setToRandomValues(); joint_state_group->getVariableValues(jnt_array_vector); for(std::size_t i=0; i < dimension_; ++i) jnt_array(i) = jnt_array_vector[i]; }
static void printName( char *name, struct stat * st, int isLastEntry, int justTheName ) { char * tag; tag = "???"; if( !justTheName ) { printf( "%s%c-- %s", prefix, isLastEntry ? '\\' : '+', name ); } else { printf( "%s", name ); } if( combo_sw ) { static char const sep[] = ":"; char const * s; s = ""; printf( "[" ); if( combo_sw & Iwanna_perms ) { printf( "%s%04o", s, st->st_mode & ~S_IFMT ); s = sep; } if( combo_sw & Iwanna_user ) { printf( "%s%s", s, getUserName( st->st_uid ) ); s = sep; } if( combo_sw & Iwanna_group ) { printf( "%s%s", s, getGroupName( st->st_gid ) ); s = sep; } if( combo_sw & Iwanna_size ) { typedef struct { char * const name; off_t const divisor; } Divisors_t; static Divisors_t const divs[] = { { "G", 1024UL * 1024UL * 1024UL }, { "M", 1024UL * 1024UL }, { "K", 1024UL }, { NULL } }; Divisors_t const * d; for( d = divs; d->name; ++d ) { if( st->st_size >= d->divisor ) { break; } } if( d->name ) { double a; a = (double) st->st_size / (double) d->divisor; printf( "%s%.0f%s", s, a, d->name ); } else { printf( "%s%llu", s, (unsigned long long ) st->st_size ); } s = sep; } printf( "]" ); } if( F_sw ) { if( S_ISREG( st->st_mode ) ) { if( st->st_mode & (S_IXUSR | S_IXGRP | S_IXOTH ) ) { tag = "*"; } else { tag = ""; } } if( S_ISDIR( st->st_mode ) ) { tag = "/"; } else if( S_ISCHR( st->st_mode ) ) { tag = "{c}"; } else if( S_ISBLK( st->st_mode ) ) { tag = "{b}"; } else if( S_ISFIFO( st->st_mode ) ) { tag = "|"; } else if( S_ISSOCK( st->st_mode ) ) { tag = "%"; } else if( S_ISLNK( st->st_mode ) ) { static char target[ PATH_MAX + 1 + 4 ]; ssize_t len; strcpy( target, " -> " ); len = readlink( name, target + 4, PATH_MAX ); if( len < 0 ) { tag = "->"; } else { target[ 4 + len ] = '\0'; tag = target; } } else if( st->st_mode & (S_IXUSR | S_IXGRP | S_IXOTH ) ) { tag = "*"; } printf( "%s", tag ); } if( S_ISDIR( st->st_mode ) ) { printf( "\n" ); } }
gkParticleObject* gkScene::createParticleObject(const gkHashedString& name) { if (m_objects.find(name) != GK_NPOS) { gkPrintf("Scene: Duplicate object '%s' found\n", name.str().c_str()); return 0; } gkParticleObject* gobj = gkGameObjectManager::getSingleton().createParticleObject(gkResourceName(name, getGroupName())); addObject(gobj); return gobj; }
gkMesh* gkScene::createMesh(const gkHashedString& name) { GK_ASSERT(m_meshManager); return (gkMesh*)m_meshManager->create(gkResourceName(name, getGroupName())); }
bool CHOMPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { moveit_msgs::MotionPlanDetailedResponse res_msg; if (chomp_interface_->solve(planning_scene_, request_, chomp_interface_->getParams(), res_msg)) { res.trajectory_.resize(1); res.trajectory_[0] = robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); moveit::core::RobotState start_state(robot_model_); robot_state::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); res.description_.push_back("plan"); res.processing_time_ = res_msg.processing_time; res.error_code_ = res_msg.error_code; return true; } else { res.error_code_ = res_msg.error_code; return false; } }
void gkScene::postCreateInstanceImpl(void) { #ifdef OGREKIT_USE_LUA gkLuaScript* script = gkLuaManager::getSingleton().getByName<gkLuaScript>(gkResourceName(DEFAULT_STARTUP_LUA_FILE, getGroupName())); if (script) script->execute(); #endif }
std::string DockItem::getDesktopFileName() { return getGroupName() + ".desktop"; }
//VOXOX CHANGE - Add support for ItemList style and change paint structure. void QtUMGroup::paintForeground(QPainter * painter, const QStyleOptionViewItem & option) { QMutexLocker locker(&_mutex); bool isGroupSelected = false;//VOXOX CHANGE by Rolando - 2009.09.03 bool isGroupBlinking = false;//VOXOX CHANGE by Rolando - 2009.09.08 QtUMItemListStyle * itemListStyle = QtUMItemListStyle::getInstance();//VOXOX CHANGE by Rolando - 2009.09.07 isGroupBlinking = isBlinking();//VOXOX CHANGE by Rolando - 2009.09.03 isGroupSelected = _isSelected;//VOXOX CHANGE by Rolando - 2009.10.01 if(isGroupSelected)//VOXOX CHANGE by Rolando - 2009.09.04 { painter->setPen(itemListStyle->getSelectedGroupFontColor());//VOXOX CHANGE by Rolando - 2009.09.04 } else { if(isGroupBlinking){ if(_blinkNow){//VOXOX CHANGE by Rolando - 2009.10.01 painter->setPen(itemListStyle->getGroupBlinkingFontColor());//VOXOX CHANGE by Rolando - 2009.10.01 } else{ painter->setPen(itemListStyle->getNonSelectedGroupFontColor());//VOXOX CHANGE by Rolando - 2009.10.01 } } else{ painter->setPen(itemListStyle->getNonSelectedGroupFontColor());//VOXOX CHANGE by Rolando - 2009.09.04 } } painter->drawPixmap(option.rect.left(),option.rect.top(),getGroupBackGround(option.rect,option, isGroupSelected, isGroupBlinking)); QFont font = option.font; painter->setFont(font); QPixmap px; if (option.state & QStyle::State_Open) { px = itemListStyle->getGroupPixmapOpen(); } else { px = itemListStyle->getGroupPixmapClose(); } int x = option.rect.left(); QRect r = option.rect; painter->drawPixmap(x, r.top() + 3, px); _posXArrowButton = x;//VOXOX CHANGE by Rolando - 2009.09.29 _posYArrowButton = r.top() + 3;//VOXOX CHANGE by Rolando - 2009.09.29 _widthArrowButton = px.width();//VOXOX CHANGE by Rolando - 2009.09.29 _heightArrowButton = px.height();//VOXOX CHANGE by Rolando - 2009.09.29 x += px.width() + 3; r.setLeft(x); int y = ((r.bottom()-r.top())-QFontMetrics(font).height())/2; r.setTop(y + r.top()); painter->drawText(r, Qt::AlignLeft, getGroupName(), 0); //VOXOX CHANGE by Rolando - 2009.08.25 - getting corresponding close button QPixmap closeButtonPixmap; if ((option.state & QStyle::State_Selected) == QStyle::State_Selected) { closeButtonPixmap = QPixmap(":/pics/chat/umtreewidget/btn_close_selected.png");//VOXOX CHANGE by Rolando - 2009.08.25 painter->setPen(_fontColor);//VOXOX CHANGE by Rolando - 2009.09.07 } else { closeButtonPixmap = QPixmap(":/pics/chat/umtreewidget/btn_close_nonselected.png");//VOXOX CHANGE by Rolando - 2009.08.25 painter->setPen(_fontColor);//VOXOX CHANGE by Rolando - 2009.09.07 } //VOXOX CHANGE by Rolando - 2009.08.25 - painting the close button _posXCloseButton = r.right() - closeButtonPixmap.width() - 5;//VOXOX CHANGE by Rolando - 2009.09.16 int centeredCloseButton_y = ((r.bottom() - r.top()) - closeButtonPixmap.size().height()) / 2; painter->drawPixmap(_posXCloseButton, r.top() + centeredCloseButton_y, closeButtonPixmap); _posYCloseButton = r.top() + centeredCloseButton_y; _widthCloseButton = closeButtonPixmap.width();//VOXOX CHANGE by Rolando - 2009.08.27 _heightCloseButton = closeButtonPixmap.height();//VOXOX CHANGE by Rolando - 2009.08.27 }
// ------------------------------------------------------------------- // File parsing method. void ObjFileParser::parseFile() { if (m_DataIt == m_DataItEnd) return; // only update every 100KB or it'll be too slow const unsigned int updateProgressEveryBytes = 100 * 1024; unsigned int progressCounter = 0; const unsigned int bytesToProcess = std::distance(m_DataIt, m_DataItEnd); const unsigned int progressTotal = 3 * bytesToProcess; const unsigned int progressOffset = bytesToProcess; unsigned int processed = 0; DataArrayIt lastDataIt = m_DataIt; while (m_DataIt != m_DataItEnd) { // Handle progress reporting processed += std::distance(lastDataIt, m_DataIt); lastDataIt = m_DataIt; if (processed > (progressCounter * updateProgressEveryBytes)) { progressCounter++; m_progress->UpdateFileRead(progressOffset + processed*2, progressTotal); } // parse line switch (*m_DataIt) { case 'v': // Parse a vertex texture coordinate { ++m_DataIt; if (*m_DataIt == ' ' || *m_DataIt == '\t') { size_t numComponents = getNumComponentsInLine(); if (numComponents == 3) { // read in vertex definition getVector3(m_pModel->m_Vertices); } else if (numComponents == 6) { // read vertex and vertex-color getTwoVectors3(m_pModel->m_Vertices, m_pModel->m_VertexColors); } } else if (*m_DataIt == 't') { // read in texture coordinate ( 2D or 3D ) ++m_DataIt; getVector( m_pModel->m_TextureCoord ); } else if (*m_DataIt == 'n') { // Read in normal vector definition ++m_DataIt; getVector3( m_pModel->m_Normals ); } } break; case 'p': // Parse a face, line or point statement case 'l': case 'f': { getFace(*m_DataIt == 'f' ? aiPrimitiveType_POLYGON : (*m_DataIt == 'l' ? aiPrimitiveType_LINE : aiPrimitiveType_POINT)); } break; case '#': // Parse a comment { getComment(); } break; case 'u': // Parse a material desc. setter { getMaterialDesc(); } break; case 'm': // Parse a material library or merging group ('mg') { if (*(m_DataIt + 1) == 'g') getGroupNumberAndResolution(); else getMaterialLib(); } break; case 'g': // Parse group name { getGroupName(); } break; case 's': // Parse group number { getGroupNumber(); } break; case 'o': // Parse object name { getObjectName(); } break; default: { m_DataIt = skipLine<DataArrayIt>( m_DataIt, m_DataItEnd, m_uiLine ); } break; } } }
//VOXOX CHANGE - Add support for ItemList style and change paint structure. void QtUMItem::paintForeground(QPainter * painter, const QStyleOptionViewItem & option) { QMutexLocker locker(&_mutex); QtContactPixmap * spx = QtContactPixmap::getInstance(); QtUMItemListStyle * itemListStyle = QtUMItemListStyle::getInstance(); //VOXOX CHANGE by Rolando - 2009.08.25 - getting corresponding close button QPixmap closeButtonPixmap; if ((option.state & QStyle::State_Selected) == QStyle::State_Selected) { closeButtonPixmap = QPixmap(":/pics/chat/umtreewidget/btn_close_selected.png");//VOXOX CHANGE by Rolando - 2009.08.25 painter->setPen(_fontColor);//VOXOX CHANGE by Rolando - 2009.09.07 } else { closeButtonPixmap = QPixmap(":/pics/chat/umtreewidget/btn_close_nonselected.png");//VOXOX CHANGE by Rolando - 2009.08.25 painter->setPen(_fontColor);//VOXOX CHANGE by Rolando - 2009.09.07 } QRect painterRect = option.rect; if(!isAGroupItem()){ //VOXOX CHANGE by Rolando - 2009.08.25 - painting the close button _posXCloseButton = painterRect.right() - closeButtonPixmap.width() - 5;//VOXOX CHANGE by Rolando - 2009.09.16 int centeredCloseButton_y = ((painterRect.bottom() - painterRect.top()) - closeButtonPixmap.size().height()) / 2; painter->drawPixmap(_posXCloseButton, painterRect.top() + centeredCloseButton_y, closeButtonPixmap); _posYCloseButton = painterRect.top() + centeredCloseButton_y; _widthCloseButton = closeButtonPixmap.width();//VOXOX CHANGE by Rolando - 2009.08.27 _heightCloseButton = closeButtonPixmap.height();//VOXOX CHANGE by Rolando - 2009.08.27 } else{ _posXCloseButton = painterRect.right();//VOXOX CHANGE by Rolando - 2009.09.25 } //painting the status icon QPixmap normalPx = getCurrentStatusPixmap();//VOXOX CHANGE by Rolando - 2009.10.26 _posXNetworkStatus = _posXCloseButton - normalPx.width() - 4;//VOXOX CHANGE by Rolando - 2009.10.26 _posXNetworkButton = _posXNetworkStatus; int centeredPx_y = ((painterRect.bottom() - painterRect.top()) - normalPx.size().height()) / 2;//VOXOX CHANGE by Rolando - 2009.10.26 _posYNetworkButton = painterRect.top() + centeredPx_y;//VOXOX CHANGE by Rolando - 2009.10.26 painter->drawPixmap(_posXNetworkStatus, painterRect.top() + centeredPx_y, normalPx);//VOXOX CHANGE by Rolando - 2009.10.26 _widthNetworkButton = normalPx.size().width();//VOXOX CHANGE by Rolando - 2009.10.21 _heightNetworkButton = normalPx.size().height();//VOXOX CHANGE by Rolando - 2009.10.21 std::string foregroundPixmapData = getAvatarData().toStdString(); QPixmap avatar; avatar.loadFromData ((uchar*) foregroundPixmapData.c_str(), foregroundPixmapData.size()); avatar = avatar.scaled ( AVATAR_HEIGHT, AVATAR_WIDTH, Qt::KeepAspectRatio, Qt::SmoothTransformation) ; if(getGroupName() == ""){//VOXOX CHANGE by Rolando - 2009.08.31 painter->drawPixmap(CONTACT_MARGIN, painterRect.top() + (painterRect.height() - AVATAR_HEIGHT) /2, avatar);//VOXOX CHANGE by Rolando - 2009.08.31 painterRect.setLeft(CONTACT_MARGIN+AVATAR_WIDTH+AVATAR_TEXT_MARGIN);//VOXOX CHANGE by Rolando - 2009.08.31 } else{ painter->drawPixmap(CONTACT_MARGIN_IN_GROUP, painterRect.top() + (painterRect.height() - AVATAR_HEIGHT) /2, avatar);//VOXOX CHANGE by Rolando - 2009.08.31 painterRect.setLeft(CONTACT_MARGIN_IN_GROUP+AVATAR_WIDTH+AVATAR_TEXT_MARGIN);//VOXOX CHANGE by Rolando - 2009.08.31 } // Draw the text painter->setFont(option.font); // Center the text vertically QRect textRect = painterRect; int textY = painterRect.top()+AVATAR_MARGIN_TOP+3; textRect.setTop(textY); QString text = fixHtmlString(getDisplayName()); text = verifyText(textRect,option.font,text); painter->drawText(textRect, Qt::AlignLeft, text, 0); }
bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses, const std::vector<double> &ik_seed_state, double timeout, const std::vector<double> &consistency_limits, std::vector<double> &solution, const IKCallbackFn &solution_callback, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options) const { // Check if active if(!active_) { ROS_ERROR_NAMED("srv","kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Check if seed state correct if(ik_seed_state.size() != dimension_) { ROS_ERROR_STREAM_NAMED("srv","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Check that we have the same number of poses as tips if (tip_frames_.size() != ik_poses.size()) { ROS_ERROR_STREAM_NAMED("srv","Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames (" << tip_frames_.size() << ") in searchPositionIK"); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Create the service message moveit_msgs::GetPositionIK ik_srv; ik_srv.request.ik_request.avoid_collisions = true; ik_srv.request.ik_request.group_name = getGroupName(); // Copy seed state into virtual robot state and convert into moveit_msg robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state); moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state); // Load the poses into the request in difference places depending if there is more than one or not geometry_msgs::PoseStamped ik_pose_st; ik_pose_st.header.frame_id = base_frame_; if (tip_frames_.size() > 1) { // Load into vector of poses for (std::size_t i = 0; i < tip_frames_.size(); ++i) { ik_pose_st.pose = ik_poses[i]; ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st); ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]); } } else { ik_pose_st.pose = ik_poses[0]; // Load into single pose value ik_srv.request.ik_request.pose_stamped = ik_pose_st; ik_srv.request.ik_request.ik_link_name = getTipFrames()[0]; } ROS_DEBUG_STREAM_NAMED("srv","Calling service: " << ik_service_client_->getService() ); if (ik_service_client_->call(ik_srv)) { // Check error code error_code.val = ik_srv.response.error_code.val; if(error_code.val != error_code.SUCCESS) { ROS_DEBUG_NAMED("srv","An IK that satisifes the constraints and is collision free could not be found."); switch (error_code.val) { // Debug mode for failure: ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request); ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution); case moveit_msgs::MoveItErrorCodes::FAILURE: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: FAILURE"); break; case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: NO IK SOLUTION"); break; default: ROS_ERROR_STREAM_NAMED("srv","Service failed with with error code: " << error_code.val); } return false; } } else { ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService() ); error_code.val = error_code.FAILURE; return false; } // Convert the robot state message to our robot_state representation if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_)) { ROS_ERROR_STREAM_NAMED("srv","An error occured converting recieved robot state message into internal robot state."); error_code.val = error_code.FAILURE; return false; } // Get just the joints we are concerned about in our planning group robot_state_->copyJointGroupPositions(joint_model_group_, solution); // Run the solution callback (i.e. collision checker) if available if (!solution_callback.empty()) { ROS_DEBUG_STREAM_NAMED("srv","Calling solution callback on IK solution"); // hack: should use all poses, not just the 0th solution_callback(ik_poses[0], solution, error_code); if(error_code.val != error_code.SUCCESS) { switch (error_code.val) { case moveit_msgs::MoveItErrorCodes::FAILURE: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: FAILURE"); break; case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: NO IK SOLUTION"); break; default: ROS_ERROR_STREAM_NAMED("srv","IK solution callback failed with with error code: " << error_code.val); } return false; } } ROS_INFO_STREAM_NAMED("srv","IK Solver Succeeded!"); return true; }
bool KDLKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state, const std::vector<double> &consistency_limits, const KDL::JntArray& solution) const { std::vector<double> seed_state_vector(dimension_), solution_vector(dimension_); for(std::size_t i = 0; i < dimension_; ++i) { seed_state_vector[i] = seed_state(i); solution_vector[i] = solution(i); } robot_state::JointStateGroup* joint_state_group = kinematic_state_->getJointStateGroup(getGroupName()); robot_state::JointStateGroup* joint_state_group_2 = kinematic_state_2_->getJointStateGroup(getGroupName()); joint_state_group->setVariableValues(seed_state_vector); joint_state_group_2->setVariableValues(solution_vector); const std::vector<robot_state::JointState*>& joint_state_vector = joint_state_group->getJointStateVector(); const std::vector<robot_state::JointState*>& joint_state_vector_2 = joint_state_group_2->getJointStateVector(); for(std::size_t i = 0; i < joint_state_vector.size(); ++i) { if(joint_state_vector[i]->distance(joint_state_vector_2[i]) > consistency_limits[i]) return false; } return true; }
void gkScene::destroyInstanceImpl(void) { //if (m_objects.empty()) // return; if (m_navMeshData.get()) m_navMeshData->destroyInstances(); #ifdef OGREKIT_USE_LUA // Free scripts gkLuaManager::getSingleton().decompileGroup(getGroupName()); #endif m_cameras.clear(true); m_lights.clear(true); m_staticControllers.clear(true); gkGroupManager::getSingleton().destroyGameObjectInstances(this); // Destroy all batched geometry if (gkEngine::getSingleton().getUserDefs().buildStaticGeometry) gkGroupManager::getSingleton().destroyStaticBatches(this); gkGameObjectSet::Iterator it = m_instanceObjects.iterator(); while (it.hasMoreElements()) { gkGameObject* gobj = it.getNext(); GK_ASSERT(gobj->isInstanced()); gobj->destroyInstance(); // gkGameObjectManager::getSingleton().destroy(gobj); } m_instanceObjects.clear(true); // Free cloned. destroyClones(); // Remove any pending endObjects(); if (m_physicsWorld) { delete m_physicsWorld; m_physicsWorld = 0; } if (m_debugger) { delete m_debugger; m_debugger = 0; } if (m_skybox) { delete m_skybox; m_skybox = 0; } m_startCam = 0; m_limits = gkBoundingBox::BOX_NULL; if (m_manager) { #if OGREKIT_USE_RTSHADER_SYSTEM Ogre::RTShader::ShaderGenerator::getSingleton().removeSceneManager(m_manager); #endif Ogre::Root::getSingleton().destroySceneManager(m_manager); m_manager = 0; } if (m_viewport) { getDisplayWindow()->removeViewport(m_viewport); m_viewport = 0; } gkLogicManager::getSingleton().notifySceneInstanceDestroyed(); gkWindowSystem::getSingleton().clearStates(); gkEngine::getSingleton().unregisterActiveScene(this); #ifdef OGREKIT_OPENAL_SOUND gkSoundManager::getSingleton().stopAllSounds(); #endif //_eraseAllObjects(); }
// ------------------------------------------------------------------- void ObjFileParser::parseFile() { if (m_DataIt == m_DataItEnd) return; while (m_DataIt != m_DataItEnd) { switch (*m_DataIt) { case 'v': // Parse a vertex texture coordinate { ++m_DataIt; if (*m_DataIt == ' ') { // Read in vertex definition getVector3(m_pModel->m_Vertices); } else if (*m_DataIt == 't') { // Read in texture coordinate (2D) ++m_DataIt; getVector2(m_pModel->m_TextureCoord); } else if (*m_DataIt == 'n') { // Read in normal vector definition ++m_DataIt; getVector3( m_pModel->m_Normals ); } } break; case 'f': // Parse a face { getFace(); } break; case '#': // Parse a comment { getComment(); } break; case 'u': // Parse a material desc. setter { getMaterialDesc(); } break; case 'm': // Parse a material library { getMaterialLib(); } break; case 'g': // Parse group name { getGroupName(); } break; case 's': // Parse group number { getGroupNumber(); } break; case 'o': // Parse object name { getObjectName(); } break; default: { m_DataIt = skipLine<DataArrayIt>( m_DataIt, m_DataItEnd, m_uiLine ); } break; } } }
void gkAnimationSequence::addItem(const gkHashedString &animation, const akScalar& start, const akScalar& end, const akScalar& blendin, const akScalar& blendout) { gkAnimation* anim = gkAnimationManager::getSingleton().getAnimation(gkResourceName(animation, getGroupName())); if (anim) addItem(anim->getInternal(), start, end, blendin, blendout); }
// ------------------------------------------------------------------- // File parsing method. void ObjFileParser::parseFile() { if (m_DataIt == m_DataItEnd) return; while (m_DataIt != m_DataItEnd) { switch (*m_DataIt) { case 'v': // Parse a vertex texture coordinate { ++m_DataIt; if (*m_DataIt == ' ' || *m_DataIt == '\t') { // read in vertex definition getVector3(m_pModel->m_Vertices); } else if (*m_DataIt == 't') { // read in texture coordinate ( 2D or 3D ) ++m_DataIt; getVector( m_pModel->m_TextureCoord ); } else if (*m_DataIt == 'n') { // Read in normal vector definition ++m_DataIt; getVector3( m_pModel->m_Normals ); } } break; case 'p': // Parse a face, line or point statement case 'l': case 'f': { getFace(*m_DataIt == 'f' ? aiPrimitiveType_POLYGON : (*m_DataIt == 'l' ? aiPrimitiveType_LINE : aiPrimitiveType_POINT)); } break; case '#': // Parse a comment { getComment(); } break; case 'u': // Parse a material desc. setter { getMaterialDesc(); } break; case 'm': // Parse a material library or merging group ('mg') { if (*(m_DataIt + 1) == 'g') getGroupNumberAndResolution(); else getMaterialLib(); } break; case 'g': // Parse group name { getGroupName(); } break; case 's': // Parse group number { getGroupNumber(); } break; case 'o': // Parse object name { getObjectName(); } break; default: { m_DataIt = skipLine<DataArrayIt>( m_DataIt, m_DataItEnd, m_uiLine ); } break; } } }
//---------------------------------------------------------------------------------- bool PropertyManager::isPropertyRegisteredByFullname(const std::string& fullname) const { // extract the group name return isPropertyRegistered(getPropertyName(fullname), getGroupName(fullname)); }