void total_bill(customer *c, date *de1, date *de2) {
int total;
int adv = c->advance;
int n = stay(de1, de2);
if(c->r_type == 1)
total = n * 350;
if(c->r_type == 2)
total = n * 600;
c->total_bill = (total - adv);
}
bool turn(Labyrinth &map) {
take_objects_from_cell(map, map.player[map.current_player]);
if (DEBUG)
print_debug(map);
user_message(map.player[map.current_player].name + ", it's your turn");
string message = map.player[map.current_player].name + ", enter what you want (go, bomb, shoot, knife, suicide, stay, leave";
if (SERVER)
message += ")";
else
message += ", save)";
user_message(message);
if (SERVER)
server.clear_user_messages(map.current_player);
string s = read_user_command(map.current_player);
if (s == "leave")
return leave_game(map, map.player[map.current_player]);
if (s == "suicide")
return suicide(map, map.player[map.current_player]);
if (s == "knife")
return use_knife(map, map.player[map.current_player]);
if (s == "bomb")
return bomb(map, map.player[map.current_player]);
if (s == "go")
return go(map, map.player[map.current_player]);
if (s == "shoot")
return shoot(map, map.player[map.current_player]);
if (s == "stay")
return stay(map.player[map.current_player]);
if (s == "save" && !SERVER) {
save_game(map);
return false;
}
if (s == "winthemall") {
if (is_developer(map.player[map.current_player])) {
user_message("Okay, master");
finish_game(map);
return true;
}
}
user_message(map.player[map.current_player].name + ", you entered incorrect command! Try again, if you can!");
return false;
}
void checkin(list *l, customer *c, date *de1, date *de2) {
int n;
append(l, c, de1, de2);
int t = c->r_type;
printf("Your booking is done for the ");
if(t == 1) {
printf("single room\n");
(single)--;
}
if(t == 2) {
printf("double room\n");
(doub)--;
}
n = stay(de1, de2);
printf("Hope you will enjoy the stay of '%d' days\n", n);
printf("THANK YOU\n");
(total_rooms)--;
}
void stay() //Function for when user selects 'Stay'
{
dealer(); //If stay selected, dealer continues going
if(dealer_total>=17)
{
if(player_total>=dealer_total) //If player's total is more than dealer's total, win
{
printf("\nUnbelievable! You Win!\n");
won = won+1;
cash = cash+bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
if(player_total<dealer_total) //If player's total is less than dealer's total, loss
{
printf("\nDealer Has the Better Hand. You Lose.\n");
loss = loss+1;
cash = cash - bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
if(dealer_total>21) //If dealer's total is more than 21, win
{
printf("\nUnbelievable! You Win!\n");
won = won+1;
cash = cash+bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
}
else
{
stay();
}
} // End Function
/*
Function prototype:
void Dealer::doubleDown(Hand);
Function description:
Function allow one hit and then has to stay
Example:
dealer.doubleDown(&playerHand);
Pre-conditions: Hand must exist
Post-conditions: player gets one card, then dealer follows house rules to get cards.
*/
void Dealer::doubleDown(Hand *hand) {
//Hit once, then has to stay.
hit(hand);
stay();
}
void GroceryDashKeyEventHandler::handleKeyEvents(Game *game){
// WE CAN QUERY INPUT TO SEE WHAT WAS PRESSED
GameInput *input = game->getInput();
// LET'S GET THE PLAYER'S PHYSICAL PROPERTIES, IN CASE WE WANT TO CHANGE THEM
GameStateManager *gsm = game->getGSM();
TopDownSprite *player = gsm->getSpriteManager()->getPlayer();
b2Body *playerBody = game->getGSM()->getSpriteManager()->playerBody;
PhysicalProperties *pp = player->getPhysicalProperties();
Viewport *viewport = game->getGUI()->getViewport();
GroceryDashDataLoader* dataLoader = dynamic_cast<GroceryDashDataLoader*>(game->getDataLoader());
if (!gsm->isGameInProgress()){
if (input->isKeyDownForFirstTime(ESC_KEY)){
if (gsm->isAtInGameHelp() == true){
gsm->goToPauseMenu();
}
else{
if (gsm->isAtSettingsScreen() == true || gsm->isAtAboutScreen() == true || gsm->isAtAchievementsScreen() == true
|| gsm->isAtHelpScreen() == true || gsm->isAtLevelSelect() == true || gsm->isAtStatsScreen() == true){
gsm->goToMainMenu();
}
}
}
}//BRACKET -- !isGameInProgress
if (gsm->isAtPauseMenu()){
if (input->isKeyDownForFirstTime(R_KEY))
{
gsm->goToGame();
}
}
if (gsm->isGameInProgress())
{
if (input->isKeyDownForFirstTime(P_KEY))
{
gsm->goToPauseMenu();
}
if (input->isKeyDownForFirstTime(L_KEY))
{
if ( gsm->isAtItemList() == false){
gsm->goToItemList();
}
else{
gsm->goToGame();
}
}
if (input->isKeyDownForFirstTime(C_KEY))
{
if (gsm->isAtCartScreen() == false){
gsm->goToCartScreen();
}
else{
gsm->goToGame();
}
}
if (input->isKeyDownForFirstTime(M_KEY)){
runScript(game);
}
// ARROW KEYS PRESSES WILL CONTROL THE PLAYER
// THE USER MOVES VIA MOUSE BUTTON PRESSES
if (input->isKeyDown(MOVE_LEFT_CONTROL) && gsm->isPlayerControlDisabled() == false && dataLoader->getIsPayingCashier() == false)
{
player->clearPath();
b2Vec2 moveLeft(-PLAYER_SPEED, 0.0f);
playerBody->ApplyForceToCenter(moveLeft, true);
playerBody->SetTransform(playerBody->GetPosition(), (1.0f * PI));
}
else if (input->isKeyDown(MOVE_RIGHT_CONTROL) && gsm->isPlayerControlDisabled() == false && dataLoader->getIsPayingCashier() == false)
{
player->clearPath();
b2Vec2 moveRight(PLAYER_SPEED, 0.0f);
playerBody->ApplyForceToCenter(moveRight, true);
playerBody->SetTransform(playerBody->GetPosition(), (0.0f * PI));
}
else if (input->isKeyDown(MOVE_UP_CONTROL) && gsm->isPlayerControlDisabled() == false && dataLoader->getIsPayingCashier() == false)
{
player->clearPath();
b2Vec2 moveUp(0.0f, -PLAYER_SPEED);
playerBody->ApplyForceToCenter(moveUp, true);
playerBody->SetTransform(playerBody->GetPosition(), (1.5f * PI));
}
else if (input->isKeyDown(MOVE_DOWN_CONTROL) && gsm->isPlayerControlDisabled() == false && dataLoader->getIsPayingCashier() == false)
{
player->clearPath();
b2Vec2 moveDown(0.0f, PLAYER_SPEED);
playerBody->ApplyForceToCenter(moveDown, true);
playerBody->SetTransform(playerBody->GetPosition(), (0.5f * PI));
}
else if (input->isKeyDownForFirstTime(G_KEY))
{
viewport->toggleDebugView();
game->getGraphics()->toggleDebugTextShouldBeRendered();
}
else if (input->isKeyDownForFirstTime(H_KEY))
{
game->getGraphics()->toggleB2DrawDebugShouldBeRendered();
}
else
{
b2Vec2 stay(0.0f, 0.0f);
playerBody->ApplyForceToCenter(stay, false);
}
if (input->isKeyDownForFirstTime(ESC_KEY) && (gsm->isPlayerAtShelf() == true || gsm->isAtItemList() == true)){
gsm->goToGame();
}
if (gsm->isPlayerAtShelf() == true){
// LEAVE THE SHELF SCREEN without TAKING ANY ITEMS
if (input->isKeyDownForFirstTime(ESC_KEY)){
gsm->goToGame();
}
int isItemTaken = false;
if (input->isKeyDownForFirstTime(NUM_1) ^ input->isKeyDownForFirstTime(NUMPAD_1)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 1);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 1, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_2) ^ input->isKeyDownForFirstTime(NUMPAD_2)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 2);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 2, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_3) ^ input->isKeyDownForFirstTime(NUMPAD_3)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 3);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 3, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_4) ^ input->isKeyDownForFirstTime(NUMPAD_4)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 4);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 4, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_5) ^ input->isKeyDownForFirstTime(NUMPAD_5)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 5);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 5, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_6) ^ input->isKeyDownForFirstTime(NUMPAD_6)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 6);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 6, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_7) ^ input->isKeyDownForFirstTime(NUMPAD_7)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 7);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 7, isItemTaken);
gsm->goToGame();
}
else{
if (input->isKeyDownForFirstTime(NUM_8) ^ input->isKeyDownForFirstTime(NUMPAD_8)){
isItemTaken = dataLoader->takeItemFromShelf(game, gsm->getGameStateShelfAsNumber(), 8);
runScript_LookUpShelfItemInfo(game, gsm->getGameStateShelfAsNumber(), 8, isItemTaken);
gsm->goToGame();
}
else{
//DO NOTHING
} } } } } } } }
}
bool viewportMoved = false;
float viewportVx = 0.0f;
float viewportVy = 0.0f;
if (input->isKeyDown(VIEWPORT_UP_CONTROL) && gsm->isPlayerControlDisabled() == false)
{
viewportVy -= MAX_VIEWPORT_AXIS_VELOCITY;
viewportMoved = true;
}
if (input->isKeyDown(VIEWPORT_DOWN_CONTROL) && gsm->isPlayerControlDisabled() == false)
{
viewportVy += MAX_VIEWPORT_AXIS_VELOCITY;
viewportMoved = true;
}
if (input->isKeyDown(VIEWPORT_LEFT_CONTROL) && gsm->isPlayerControlDisabled() == false)
{
viewportVx -= MAX_VIEWPORT_AXIS_VELOCITY;
viewportMoved = true;
}
if (input->isKeyDown(VIEWPORT_RIGHT_CONTROL) && gsm->isPlayerControlDisabled() == false)
{
viewportVx += MAX_VIEWPORT_AXIS_VELOCITY;
viewportMoved = true;
}
if (!viewportMoved){
b2Vec2 playerPos = playerBody->GetPosition();
float playerX = playerPos.x * SCALING_FACTOR;
float playerY = playerPos.y * SCALING_FACTOR;
int SCREEN_WIDTH = 1024;
int SCREEN_HEIGHT = 768;
if (viewport->getViewportX() + (viewport->getViewportWidth() * 15 / 32) < playerX){
viewportVx += MAX_VIEWPORT_AXIS_VELOCITY *
abs(viewport->getViewportX() + (viewport->getViewportWidth() / 2) - playerX) / (SCREEN_WIDTH / 2);
viewportMoved = true;
}
if (viewport->getViewportX() + (viewport->getViewportWidth() * 17 / 32) > playerX){
viewportVx -= MAX_VIEWPORT_AXIS_VELOCITY *
abs(viewport->getViewportX() + (viewport->getViewportWidth() / 2) - playerX) / (SCREEN_WIDTH / 2);
viewportMoved = true;
}
if (viewport->getViewportY() + (viewport->getViewportHeight() * 15 / 32) < playerY){
viewportVy += MAX_VIEWPORT_AXIS_VELOCITY *
abs(viewport->getViewportY() + (viewport->getViewportHeight() / 2) - playerY) / (SCREEN_HEIGHT / 2);
viewportMoved = true;
}
if (viewport->getViewportY() + (viewport->getViewportHeight() * 17 / 32) > playerY){
viewportVy -= MAX_VIEWPORT_AXIS_VELOCITY *
abs(viewport->getViewportY() + (viewport->getViewportHeight() / 2) - playerY) / (SCREEN_HEIGHT / 2);
viewportMoved = true;
}
}
//Viewport *viewport = game->getGUI()->getViewport();
if (viewportMoved){
viewport->moveViewport((int)floor(viewportVx + 0.5f), (int)floor(viewportVy + 0.5f), game->getGSM()->getWorld()->getWorldWidth(), game->getGSM()->getWorld()->getWorldHeight());
}
/*********** CHEAT CODES LISTED BELOW *******
************************************************/
// INCREASE TIME REMAINING
if (input->isKeyDownForFirstTime(I_KEY)){
runScript_TimeCheat(game, true);
}//isIncreasingTime
// DECREASE TIME REMAINING
if (input->isKeyDownForFirstTime(O_KEY)){
runScript_TimeCheat(game, false);
}
// INCREASE SPENDING LIMIT
if (input->isKeyDownForFirstTime(Y_KEY)){
if ((dataLoader->spendingLimit + MONEY_CHEAT_INCREASE) > (dataLoader->levelItemsCost * 2) + MONEY_CHEAT_INCREASE){
dataLoader->spendingLimit = (dataLoader->levelItemsCost * 2) + MONEY_CHEAT_INCREASE;
}
else{
dataLoader->spendingLimit += MONEY_CHEAT_INCREASE;
}
}//isIncreasingSpendingLimit
// DECREASE SPENDING LIMIT
if (input->isKeyDownForFirstTime(U_KEY)){
if ((dataLoader->spendingLimit - MONEY_CHEAT_DECREASE) < dataLoader->levelItemsCost){
dataLoader->spendingLimit = dataLoader->levelItemsCost;
}
else{
dataLoader->spendingLimit -= MONEY_CHEAT_DECREASE;
}
}//isDecreasingSpendingLimit
}// BRACKET -- if (gsm->isGameInProgress())
}
void play() //Plays game
{
int p=0; // holds value of player_total
int i=1; // counter for asking user to hold or stay (aka game turns)
char choice3;
cash = cash;
cash_test();
printf("\nCash: $%d\n",cash); //Prints amount of cash user has
randcard(); //Generates random card
player_total = p + l; //Computes player total
p = player_total;
printf("\nYour Total is %d\n", p); //Prints player total
dealer(); //Computes and prints dealer total
betting(); //Prompts user to enter bet amount
while(i<=21) //While loop used to keep asking user to hit or stay at most twenty-one times
// because there is a chance user can generate twenty-one consecutive 1's
{
if(p==21) //If user total is 21, win
{
printf("\nUnbelievable! You Win!\n");
won = won+1;
cash = cash+bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
if(p>21) //If player total is over 21, loss
{
printf("\nWoah Buddy, You Went WAY over.\n");
loss = loss+1;
cash = cash - bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
if(p<=21) //If player total is less than 21, ask to hit or stay
{
printf("\n\nWould You Like to Hit or Stay?");
scanf("%c", &choice3);
while((choice3!='H') && (choice3!='h') && (choice3!='S') && (choice3!='s')) // If invalid choice entered
{
printf("\n");
printf("Please Enter H to Hit or S to Stay.\n");
scanf("%c",&choice3);
}
if((choice3=='H') || (choice3=='h')) // If Hit, continues
{
randcard();
player_total = p + l;
p = player_total;
printf("\nYour Total is %d\n", p);
dealer();
if(dealer_total==21) //Is dealer total is 21, loss
{
printf("\nDealer Has the Better Hand. You Lose.\n");
loss = loss+1;
cash = cash - bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
if(dealer_total>21) //If dealer total is over 21, win
{
printf("\nDealer Has Went Over!. You Win!\n");
won = won+1;
cash = cash+bet;
printf("\nYou have %d Wins and %d Losses. Awesome!\n", won, loss);
dealer_total=0;
askover();
}
}
if((choice3=='S') || (choice3=='s')) // If Stay, does not continue
{
printf("\nYou Have Chosen to Stay at %d. Wise Decision!\n", player_total);
stay();
}
}
i++; //While player total and dealer total are less than 21, re-do while loop
} // End While Loop
} // End Function
Player::Player(tmx::MapLoader& map, Coins& coins, EnemiesManager& enemyManager, Star& star)
: mSpriteSize(48,48)
, mIsJumping(false)
, mFrictionStart(0.95)
, mFrictionStop(0.8)
, mFrictionVertical(5)
, mIsMovingLeft(false)
, mIsMovingRight(false)
, mCollisionLeft(false)
, mCollisionRight(false)
, mCollisionUp(false)
, mCollisionDown(false)
, mPlayerDead(false)
, mLevelCompleted(false)
, mCollMarg(6)
, mAnimation()
{
mMap=↦
mCoins = &coins;
mEnemyManager = &enemyManager;
mStar = ☆
mFont.loadFromFile("Media/Fonts/Raleway.ttf");
mleftMargin = 8;
mRightMargin = 8;
mTopMargin = 0;
mBottomMargin = 5;
Animation right(sf::seconds(1.0f));
right.setMargins(mleftMargin,mRightMargin,mTopMargin,mBottomMargin);
for(int i=0;i<7;i++)
right.addFrame(sf::IntRect(mSpriteSize.y*i, mSpriteSize.y*5, mSpriteSize.x,mSpriteSize.y));
mAnimation.addAnimation("right",right);
Animation stay(sf::seconds(3.0f));
stay.setMargins(mleftMargin,mRightMargin,mTopMargin,mBottomMargin);
for(int i=0;i<4;i++)
stay.addFrame(sf::IntRect(mSpriteSize.y*i, mSpriteSize.y*2, mSpriteSize.x,mSpriteSize.y));
mAnimation.addAnimation("stay",stay);
Animation jump(sf::seconds(1.0f));
jump.setMargins(mleftMargin,mRightMargin,mTopMargin,mBottomMargin);
for(int i=0;i<6;i++)
jump.addFrame(sf::IntRect(mSpriteSize.y*i, mSpriteSize.y*7, mSpriteSize.x,mSpriteSize.y));
jump.isRepeatable(false);
mAnimation.addAnimation("jump",jump);
Animation fall(sf::seconds(1.0f));
fall.setMargins(mleftMargin,mRightMargin,mTopMargin,mBottomMargin);
for(int i=3;i<6;i++)
fall.addFrame(sf::IntRect(mSpriteSize.y*i, mSpriteSize.y*7, mSpriteSize.x,mSpriteSize.y));
fall.isRepeatable(false);
mAnimation.addAnimation("fall",fall);
Animation dead(sf::seconds(1.0f));
dead.setMargins(mleftMargin,mRightMargin,mTopMargin,mBottomMargin+7);
for(int i=0;i<3;i++)
dead.addFrame(sf::IntRect(mSpriteSize.y*i, mSpriteSize.y*8, mSpriteSize.x,mSpriteSize.y));
mAnimation.addAnimation("dead",dead);
mAnimation.playAnimation("stay");
}
void loop() {
static char deck[DECK_SIZE];
static char dealerHand[HAND_SIZE];
static char hand[HAND_SIZE];
static int deckPointer = 0;
static int playerScore = 0;
static int dealerScore = 0;
static bool first = true;
static bool checkForWinner = false;
if(first) {
generateDeck(deck);
drawHand(hand,deck, &deckPointer);
drawHand(dealerHand, deck, &deckPointer);
playerScore = computeScore(hand);
if(playerScore == BLACKJACK) {
printGameResult("Blackjack!", playerScore);
resetGame(hand, dealerHand, deck, &playerScore, &dealerScore, &deckPointer, &checkForWinner);
}
first = false;
}
if(sensorValue != analogRead(A0)) {
sensorValue = analogRead(A0);
Button button = (Button) getButton();
switch (button) {
case UP_BTN:
playerScore = hit(hand,deck, &deckPointer);
break;
case DOWN_BTN:
dealerScore = stay(dealerHand,deck, &deckPointer);
checkForWinner = true;
break;
case LEFT_BTN:
break;
case RIGHT_BTN:
break;
case SELECT_BTN:
resetGame(hand,dealerHand,deck,&playerScore,&dealerScore,&deckPointer,&checkForWinner);
break;
default:
break;
}
}
printCards(hand,1,0, false);
if(computeScore(hand) == BLACKJACK) {
printGameResult("Win!", playerScore);
resetGame(hand,dealerHand,deck,&playerScore,&dealerScore,&deckPointer,&checkForWinner);
} else if(computeScore(hand) == BUST) {
printGameResult("Bust!", computeScoreNoBust(hand));
resetGame(hand,dealerHand,deck,&playerScore,&dealerScore,&deckPointer,&checkForWinner);
}
if(checkForWinner) {
printCards(dealerHand, 0,0,false);
if(computeScore(hand) > computeScore(dealerHand)) {
printGameResult("Win!", computeScore(hand));
} else if(computeScore(hand) == computeScore(dealerHand)) {
printGameResult("Tie.", computeScore(hand));
} else {
printGameResult("Lose.", computeScore(hand));
}
resetGame(hand,dealerHand,deck,&playerScore,&dealerScore,&deckPointer,&checkForWinner);
} else {
printCards(dealerHand,0,0,true);
}
computeScore(hand) > 9 ? lcd.setCursor(14,1) : lcd.setCursor(15,1);
lcd.print(computeScore(hand));
delay(100);
}
int main(int argc, char **argv)
{
ROS_INFO("START");
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//----------------------------mavros_msgs/OverrideRCIn------------------------------------
ros::Publisher rc_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override",1);
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("/mavros/cmd/takeoff");
ros::ServiceClient land_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("/mavros/cmd/land");
ros::Publisher vel = nh.advertise<geometry_msgs::TwistStamped>
("mavros/setpoint_velocity/cmd_vel",10);
mavros_msgs::OverrideRCIn msg_override;
msg_override.channels[0]= 1500;
msg_override.channels[1]= 1500;
msg_override.channels[2]= 1500;
msg_override.channels[3]= 1500;
// msg_override.channels[4]= 1100;
// msg_override.channels[5]= 1100;
// msg_override.channels[6]= 1100;
// msg_override.channels[7]= 1100;
rc_pub.publish(msg_override);
mavros_msgs::CommandTOL srv_takeoff;
srv_takeoff.request.altitude = 2;
srv_takeoff.request.latitude = 0;
srv_takeoff.request.longitude = 0;
srv_takeoff.request.min_pitch = 0;
srv_takeoff.request.yaw = 0;
mavros_msgs::CommandTOL srv_land;
srv_land.request.altitude = 2;
srv_land.request.latitude = 0;
srv_land.request.longitude = 0;
srv_land.request.min_pitch = 0;
srv_land.request.yaw = 0;
geometry_msgs::TwistStamped twist;
twist.twist.linear.x = 0;
twist.twist.linear.y = 0;
twist.twist.linear.z = 2;
cv::namedWindow("OPENCV_WINDOW");
//------------------------------------------------------------------------------------------------------
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
//send a few setpoints before starting
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
ros::Time time = ros::Time::now();
while(ros::ok())
{
int c = getch();
if (c == 'r')
{
ROS_INFO("RC override");
while(ros::ok())
{
int x = getch();
if (x == ' ')msg_override=stay (msg_override);
else if (x == 'w')msg_override=up (msg_override);
else if (x == 's')msg_override=down (msg_override);
else if (x == 'i')msg_override=forward (msg_override);
else if (x == 'k')msg_override=backward (msg_override);
else if (x == 'j')msg_override=left (msg_override);
else if (x == 'l')msg_override=right (msg_override);
else if (x == 'q')break;
rc_pub.publish(msg_override);
ros::spinOnce();
rate.sleep();
}
}
else if (c == 'o')
{
for(int i = 50; ros::ok() && i > 0; --i){
twist = twist_stay (twist);
vel.publish(twist_stay(twist));
ros::spinOnce();
rate.sleep();
}
offb_set_mode.request.custom_mode = "OFFBOARD";
if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.success)
{
ROS_INFO("Offboard enabled");
}
while(ros::ok())
{
int x = getch();
if (x == ' ')twist = twist_stay (twist);
else if (x == 'w')twist = twist_up (twist);
else if (x == 's')twist = twist_down (twist);
else if (x == 'a')twist = twist_stay (twist);
else if (x == 'd')twist = twist_stay (twist);
else if (x == 'i')twist = twist_forward (twist);
else if (x == 'k')twist = twist_backward (twist);
else if (x == 'j')twist = twist_left (twist);
else if (x == 'l')twist = twist_right (twist);
else if (x == 'q')break;
vel.publish(twist);
ros::spinOnce();
rate.sleep();
}
}
else if (c == 'a')
{
arm_cmd.request.value = true;
if( arming_client.call(arm_cmd) &&arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
}
}
else if (c == 'd')
{
arm_cmd.request.value = false;
if( arming_client.call(arm_cmd) &&arm_cmd.response.success)
{
ROS_INFO("Vehicle disarmed");
}
}
else if (c == 'z')
{
ROS_INFO("test OK");
}
else if (c == 's')
{
offb_set_mode.request.custom_mode = "STABILIZED";
if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.success)
{
ROS_INFO("STABILIZE enabled");
}
}
else if (c == 'n')
{
offb_set_mode.request.custom_mode = "POSCTL";
if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.success)
{
ROS_INFO("pose control enabled");
}
}
else if (c == 't')
{
if(takeoff_cl.call(srv_takeoff)){
ROS_INFO("srv_takeoff send ok %d", srv_takeoff.response.success);
}else{
ROS_INFO("Failed Takeoff");
}
}
else if (c == 'l')
{
if(land_cl.call(srv_land)){
ROS_INFO("srv_land send ok %d", srv_land.response.success);
}else{
ROS_ERROR("Failed Land");
}
}
ros::spinOnce();
rate.sleep();
}
return 0;
}