quad_motors.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <string.h>
00003 #include <stdio.h>
00004 #include <math.h>
00005 #include <stdlib.h>
00006 #include <Ivy/ivy.h>
00007 #include <Ivy/ivyloop.h>
00008 #include "std_srvs/Empty.h"
00009 #include <Ivy/timer.h>
00010 
00011 /* 
00012  * Service node to Start and Stop motors.
00013  * Parameters:  AC_ID;                          
00014  */
00015 
00016 int ac_id;
00017 int i;
00018 
00019 bool STARTCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00020 {
00021         ROS_INFO("Motors ON!");
00022         //IvySendMsg ("dl DL_SETTING %d 3 0",ac_id); //TO be read by paparazzi server
00023         IvySendMsg ("ground_dl SETTING 3 %d 0.0",ac_id);
00024         sleep(4);
00025         return true;
00026 }
00027 
00028 bool STOPCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00029 {
00030         ROS_INFO("Motors OFF!");
00031         //IvySendMsg ("dl DL_SETTING %d 3 1",ac_id); //TO be read by paparazzi server
00032         IvySendMsg ("ground_dl SETTING 3 %d 1.0",ac_id);
00033         return true;
00034 }
00035 
00036 void ROSCallback (TimerId id, void *user_data, unsigned long delta) 
00037 { 
00038         ros::spinOnce();
00039         if (!ros::ok()){
00040                 for (i = 0; i < 5; i++) IvySendMsg ("ground_dl SETTING 3 %d 1.0",ac_id);
00041                 //IvySendMsg ("dl DL_SETTING %d 3 1",ac_id); //TO be read by paparazzi server
00042                 IvyStop();
00043                 exit(0);
00044         }
00045 }
00046 
00047 int main(int argc, char **argv)
00048 {
00049         // Initializing ROS
00050         ros::init(argc, argv, "quad_motors");
00051         ros::NodeHandle nh("~");
00052         
00053         // Getting ROS Parameters
00054         if (!nh.getParam("ac_id", ac_id))
00055         {
00056                 ROS_FATAL("Failed to get param 'AC_ID'");
00057                 exit(0);
00058         }
00059         
00060         // Initializing Ivy
00061         IvyInit ("quad_motors", "'Quad Motors' is READY!", 0, 0, 0, 0);
00062         IvyStart("127.255.255.255");
00063         
00064         // Initializing ROS Services
00065         ros::ServiceServer service_on = nh.advertiseService("start", STARTCallback);
00066         ros::ServiceServer service_off = nh.advertiseService("stop", STOPCallback);
00067         TimerRepeatAfter (TIMER_LOOP, 1, ROSCallback, 0);
00068         
00069         //ros::AsyncSpinner spinner(0);
00070         //spinner.start();
00071         
00072         IvyMainLoop();
00073         
00074         return 0;
00075 }


quad_motors
Author(s): Henrique
autogenerated on Mon Jan 6 2014 11:48:00