void loop(){ boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr); const unsigned X=0,Y=1,Z=2,THETA=5; // World frame boost::shared_ptr<Ravelin::Pose3d> environment_frame(new Ravelin::Pose3d()); Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Pose(*environment_frame.get()))); boost::shared_ptr<Ravelin::Pose3d> base_horizontal_frame(new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_horizontal_frame"))); Ravelin::Vector3d com(base_horizontal_frame->x.data()); OUT_LOG(logERROR) << "x = " << base_horizontal_frame->x ; Ravelin::VectorNd base_xd; ctrl->get_base_value(Pacer::Robot::velocity,base_xd); OUT_LOG(logERROR) << "xd = " << base_xd ; ///////////////////// /// SET VARIABLES /// // Command robot to walk in a direction Ravelin::VectorNd command; command.set_zero(6); double max_forward_speed = ctrl->get_data<double>(plugin_namespace+".max-forward-speed"); double max_strafe_speed = ctrl->get_data<double>(plugin_namespace+".max-strafe-speed"); double max_turn_speed = ctrl->get_data<double>(plugin_namespace+".max-turn-speed"); // if FALSE, drive like a car (x and theta) // Q: if TRUE, turn toward waypoint while stepping in direction of waypoint bool HOLONOMIC = false; ///////////////////////////////////// /// Assign WAYPOINTS in the plane /// static std::vector<Point> waypoints; std::vector<double> waypoints_vec = ctrl->get_data<std::vector<double> >(plugin_namespace+".waypoints"); // if (waypoints not inititalized) OR (waypoints changed) OR (only one waypoint) // update waypoints if(waypoints.empty() || waypoints_vec.size()/2 != waypoints.size() || waypoints_vec.size() == 2){ waypoints.clear(); for(int i=0;i<waypoints_vec.size();i+=2) waypoints.push_back(Point(waypoints_vec[i],waypoints_vec[i+1])); } ///////////////////////////// /// CHOOSE NEXT WAYPOINT /// int num_waypoints = waypoints.size(); // if waypoints are empty // do not do anything (do not update SE2 command) if(num_waypoints != 0){ Ravelin::Vector3d goto_point; static int waypoint_index = 0; static Ravelin::Vector3d next_waypoint(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame); if(waypoints.size() == 1){ waypoint_index = 0; next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame); } double distance_to_wp = (Ravelin::Origin3d(next_waypoint.data()) - Ravelin::Origin3d(com[X],com[Y],0)).norm(); double max_dist = ctrl->get_data<double>(plugin_namespace+".tolerance"); if( distance_to_wp < max_dist){ OUT_LOG(logDEBUG1) << "waypoint reached, incrementing waypoint."; OUTLOG(next_waypoint,"this_wp",logDEBUG1); OUT_LOG(logDEBUG1) << "this waypoint: " << next_waypoint; OUT_LOG(logDEBUG1) << "robot position: " << com; waypoint_index = (waypoint_index+1)% num_waypoints; if(waypoint_index == 0) exit(0); next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame); } OUT_LOG(logDEBUG1) << "num_wps = " << num_waypoints; OUT_LOG(logDEBUG1) << "distance_to_wp = " << distance_to_wp; OUT_LOG(logDEBUG1) << "waypoint_index = " << waypoint_index; Utility::visualize.push_back(Pacer::VisualizablePtr(new Pacer::Ray(next_waypoint,com,Ravelin::Vector3d(1,0.5,0)))); OUT_LOG(logDEBUG1) << "next_wp" << next_waypoint; for(int i=0;i<num_waypoints;i++){ Ravelin::Vector3d wp(waypoints[i].first,waypoints[i].second,0,environment_frame); OUT_LOG(logDEBUG1) << "\twp" << wp; Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Point(wp,Ravelin::Vector3d(1,0.5,0),0.1))); } goto_point = next_waypoint; /////////////////////// /// GO TO WAYPOINT /// // Find direction to waypoint from robot position Ravelin::Vector3d goto_direction = Ravelin::Vector3d(goto_point[X],goto_point[Y],0,environment_frame) - Ravelin::Vector3d(com[X],com[Y],0,environment_frame); goto_direction = Ravelin::Pose3d::transform_vector(base_horizontal_frame,goto_direction); goto_direction.normalize(); double angle_to_goal = atan2(goto_direction[Y],goto_direction[X]); OUT_LOG(logDEBUG) << "angle_to_goal = " << angle_to_goal; // If robot is facing toward goal already, walk in that direction if(fabs(angle_to_goal) < M_PI_4){ if(HOLONOMIC){ command[Y] = goto_direction[Y]*max_strafe_speed; } command[X] = goto_direction[X]*max_forward_speed; command[THETA] = angle_to_goal; } else { command[THETA] = Utility::sign(angle_to_goal)*max_turn_speed; if(HOLONOMIC){ command[X] = goto_direction[X]*max_forward_speed; command[Y] = goto_direction[Y]*max_strafe_speed; } else { command[X] = 0; command[Y] = 0; } } // double slow_down_tol = 0.1; // if(distance_to_wp < slow_down_tol) // command *= distance_to_wp*slow_down_tol+0.05; Ravelin::Origin3d command_SE2(command[X],command[Y],command[THETA]); ctrl->set_data<Ravelin::Origin3d>("SE2_command",command_SE2); } }
//修改积分 bool __cdecl CServerUserItem::WriteScore(tagScoreInfo & ScoreInfo, DWORD dwPlayTimeCount) { //效验参数 ASSERT(m_bAcitve==true); if (m_bAcitve==false) return false; //修改属性 switch (ScoreInfo.ScoreKind) { case enScoreKind_Win: { m_ScoreModify.lWinCount++; m_ServerUserData.UserScoreInfo.lWinCount++; break; } case enScoreKind_Lost: { m_ScoreModify.lLostCount++; m_ServerUserData.UserScoreInfo.lLostCount++; break; } case enScoreKind_Draw: { m_ScoreModify.lDrawCount++; m_ServerUserData.UserScoreInfo.lDrawCount++; break; } case enScoreKind_Flee: { m_ScoreModify.lFleeCount++; m_ServerUserData.UserScoreInfo.lFleeCount++; break; } } //修改时间 for ( int nIndex = 0; nIndex < PROPERTY_COUNT; ++nIndex ) { if ( nIndex == PROP_FOURDOLD && m_ServerUserData.PropertyInfo[PROP_DOUBLE].dwConsumedCount <= m_ServerUserData.PropertyInfo[PROP_DOUBLE].dwPropCount ) continue; if ( nIndex == PROP_BUGLE || nIndex == PROP_FLEE || nIndex == PROP_NEGAGIVE ) continue; m_ServerUserData.PropertyInfo[nIndex].dwConsumedCount += dwPlayTimeCount; } //修改积分 m_lRevenue+=ScoreInfo.lRevenue; m_ScoreModify.lScore+=ScoreInfo.lScore; m_ServerUserData.UserScoreInfo.lScore+=ScoreInfo.lScore; if(m_wServerType==GAME_GENRE_GOLD) { if ((m_ServerUserData.UserScoreInfo.lGameGold+ScoreInfo.lScore)<0) { OUTLOG(TEXT("写用户分异常,写入分后游戏分小于0,用户ID[%ld] 当前游戏分%I64d,要写入的分:%I64d,输赢统计:%I64d,当前银行分:%I64d,桌子ID:%d,椅子ID:%d,状态:%d"), m_ServerUserData.dwUserID, m_ServerUserData.UserScoreInfo.lGameGold, ScoreInfo.lScore, m_ScoreModify.lGameGold, m_ServerUserData.UserScoreInfo.lInsureScore, (int)m_ServerUserData.wTableID, (int)m_ServerUserData.wChairID, (int)m_ServerUserData.cbUserStatus); } m_ScoreModify.lGameGold+=ScoreInfo.lScore; m_ServerUserData.UserScoreInfo.lGameGold+=ScoreInfo.lScore; } //用户属性 if ((ScoreInfo.ScoreKind!=enScoreKind_Service)&&(ScoreInfo.ScoreKind!=enScoreKind_Present)) { //游戏时间 m_dwPlayTimeCount+=dwPlayTimeCount; //用户经验 m_ScoreModify.lExperience+=dwPlayTimeCount; m_ServerUserData.UserScoreInfo.lExperience+=dwPlayTimeCount; } return true; }