DroneSwarm.cpp
Go to the documentation of this file.
1 /*
2 BSD 2-Clause License
3 
4 Copyright (c) 2019, Simranjeet Singh
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice, this
11  list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
21 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
23 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
24 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
25 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28 
29 #include <ros/ros.h>
30 #include <std_msgs/String.h>
31 #include <edrone_client/edrone_services.h>
32 #include <geometry_msgs/PoseArray.h>
33 #include <sys/time.h>
34 #include <boost/thread.hpp>
35 #include <boost/chrono.hpp>
36 #include <pthread.h>
37 #include <unistd.h>
38 #include <eyantra_drone/Common.h>
39 #include <eyantra_drone/Protocol.h>
40 #include <edrone_client/edrone_msgs.h>
41 #include <stdlib.h>
42 
43 #include <string>
44 
45 #define PORT 23
46 
47 using namespace std;
48 
49 
50 bool isSocketCreate=false;
51 
54 
56 edrone_client::edrone_services service[2];
57 
58 //9 elements for drone index connected in the network
59 int userRC[9]={1500,1500,1500,1500,1000,1000,1000,1000,0};
60 
61 //vector of all string ips
62 vector <string> all_ips;
63 
64 struct ip_struct
65 {
66  int index;
67  std::string ip;
68 };
69 
70 void *createSocket(void *arg){
71 
72  struct ip_struct local_var = *(struct ip_struct *)arg;
73  //cout<<local_ip.ip;
74  isSocketCreate=com.connectMulSock(local_var.ip, local_var.index);
75  pthread_exit(NULL);
76 };
77 
78 void *writeFunction(void *threadid){
79 
80  std::vector<int> requests;
81  requests.push_back(MSP_RC);
82  requests.push_back(MSP_ATTITUDE);
83  requests.push_back(MSP_RAW_IMU);
84  requests.push_back(MSP_ALTITUDE);
85 
86  while(1)
87  {
89  pro.sendMulRequestMSP_GET_DEBUG(requests, userRC[8]);
90  usleep(22000);
91  }
92  pthread_exit(NULL);
93 }
94 
95 void *readFunction(void *threadid){
96  do
97  {
98  com.readFrame();
99  //usleep(5);
100  }
101  while(1);
102  pthread_exit(NULL);
103 }
104 
105 void *readMulFunction(void *arg){
106 
107  struct ip_struct local_var = *(struct ip_struct *)arg;
108  do
109  {
110  com.readMulFrame(local_var.index);
111  //usleep(5);
112  }
113  while(1);
114  pthread_exit(NULL);
115 }
116 
117 void *serviceFunction(void *arg){
118  struct ip_struct local_var = *(struct ip_struct *)arg;
119  while (1)
120  {
121  if (serviceClient.call(service[local_var.index]))
122  {
123  service[local_var.index].request.accX=accX;
124  service[local_var.index].request.accY=accY;
125  service[local_var.index].request.accZ=accZ;
126  service[local_var.index].request.gyroX=gyroX;
127  service[local_var.index].request.gyroY=gyroY;
128  service[local_var.index].request.gyroZ=gyroZ;
129  service[local_var.index].request.magX=magX;
130  service[local_var.index].request.magY=magY;
131  service[local_var.index].request.magZ=magZ;
132  service[local_var.index].request.roll=roll;
133  service[local_var.index].request.pitch=pitch;
134  service[local_var.index].request.yaw=yaw;
135  service[local_var.index].request.alt=alt;
136  }
137  }
138  pthread_exit(NULL);
139 }
140 
141 void Callback(const edrone_client::edrone_msgs::ConstPtr& msg){
142  userRC[0] = msg->rcRoll;
143  userRC[1] = msg->rcPitch;
144  userRC[2] = msg->rcThrottle;
145  userRC[3] = msg->rcYaw;
146  userRC[4] = msg->rcAUX1;
147  userRC[5] = msg->rcAUX2;
148  userRC[6] = msg->rcAUX3;
149  userRC[7] = msg->rcAUX4;
150  userRC[8] = msg->droneIndex;
151  // cout << "roll = " << userRC[0] << "pitch = " <<userRC[1] << "Throttle = "<< userRC[2] << endl;
152 }
153 
154 int main(int argc, char **argv){
155 
156  ros::init(argc, argv, "droneswarm");
157 
158  //add IPs here
159  all_ips.push_back("192.168.43.151");
160  //all_ips.push_back("192.168.43.249");
161  //all_ips.push_back("");
162 
163  unsigned int i = 0;
164 
165  //struct to pass to create thread
166  struct ip_struct ipStructVar;
167 
168  //Topic name. Index gets appended in for loop
169  char topic_name[] = "drone_command/ ";
170  char service_name[] = "droneService ";
171 
172  ros::NodeHandle n;
173 
174  ros::Subscriber sub[all_ips.size()];
175 
176  //create thread
177  pthread_t tds[all_ips.size()];
178  //read thread
179  pthread_t rds[all_ips.size()];
180  //write thread
181  pthread_t wds[all_ips.size()];
182  //service thread
183  pthread_t sds[all_ips.size()];
184 
185  int rc;
186 
187  for(i = 0; i < all_ips.size(); i++){
188  cout<<"Start";
189 
190  //Add an index to the topic
191  topic_name[strlen(topic_name)-1] = 0x30+i;
192  //Add an index to the service
193  service_name[strlen(service_name)-1] = 0x30+i;
194 
195  //Multiple topics is not really required since a single callback is being used
196  //and droneMsg is modified to identify the drone by index starting from 0.
197  //However, I've placed it to make it user friendly, because people are stupid!
198  sub[i] = n.subscribe(topic_name, 1000, Callback);
199 
200  //Get the IP in the IP struct
201  ipStructVar.index = i;
202  ipStructVar.ip = all_ips[i];
203 
204  //create thread
205  rc = pthread_create(&tds[i], NULL, createSocket, (void *) &ipStructVar);
206  if (rc)
207  {
208  cout << "Error:unable to create communication thread," << rc << endl;
209  exit(-1);
210  }
211 
212  pthread_join( tds[i], NULL);
213 
214  if(isSocketCreate)
215  {
216  rc = pthread_create(&wds[i], NULL, writeFunction, (void *)2);
217  if (rc)
218  {
219  cout << "Error:unable to create write thread," << rc << endl;
220  exit(-1);
221  }
222 
223  //Couldn't figure out how to get the data from drones!
224 
225  rc = pthread_create(&rds[i], NULL, readMulFunction, (void *) &ipStructVar);
226 
227  if (rc)
228  {
229  cout << "Error:unable to read create thread," << rc << endl;
230  exit(-1);
231  }
232 
233  cout<<service_name;
234 
235  serviceClient = n.serviceClient<edrone_client::edrone_services>(service_name,true);
236  rc = pthread_create(&sds[i], NULL, serviceFunction, (void *) &ipStructVar);
237 
238  if (rc)
239  {
240  cout << "Error:unable to service create thread," << rc << endl;
241  exit(-1);
242  }
243  }
244 
245  cout<<"End";
246  }
247  ros::spin();
248  return 0;
249 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::ServiceClient serviceClient
Definition: DroneSwarm.cpp:55
static const int MSP_ATTITUDE
Definition: Protocol.h:43
int i
edrone_client::edrone_services service[2]
Definition: DroneSwarm.cpp:56
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
float gyroY
Definition: Protocol.cpp:49
int userRC[9]
Definition: DroneSwarm.cpp:59
float accZ
Definition: Protocol.cpp:47
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
void * writeFunction(void *threadid)
Definition: DroneSwarm.cpp:78
void sendMulRequestMSP_SET_RAW_RC(int channels[])
Definition: Protocol.cpp:213
float accY
Definition: Protocol.cpp:46
int pitch
Definition: Protocol.cpp:41
float gyroZ
Definition: Protocol.cpp:50
ROSCPP_DECL void spin(Spinner &spinner)
static const int MSP_RC
Definition: Protocol.h:42
void * readFunction(void *threadid)
Definition: DroneSwarm.cpp:95
Communication com
Definition: DroneSwarm.cpp:52
static const int MSP_ALTITUDE
Definition: Protocol.h:44
Protocol pro
Definition: DroneSwarm.cpp:53
int main(int argc, char **argv)
Definition: DroneSwarm.cpp:154
vector< string > all_ips
Definition: DroneSwarm.cpp:62
int roll
Definition: Protocol.cpp:40
void readMulFrame(int index)
void sendMulRequestMSP_GET_DEBUG(std::vector< int > requests, int index)
Definition: Protocol.cpp:255
float alt
Definition: Protocol.cpp:54
sensor_msgs::ImagePtr msg
Definition: DroneCam.cpp:56
float magY
Definition: Protocol.cpp:52
float magZ
Definition: Protocol.cpp:53
bool connectMulSock(const std::string &ip, int index)
float magX
Definition: Protocol.cpp:51
void * readMulFunction(void *arg)
Definition: DroneSwarm.cpp:105
void * serviceFunction(void *arg)
Definition: DroneSwarm.cpp:117
std::string ip
Definition: DroneSwarm.cpp:67
bool isSocketCreate
Definition: DroneSwarm.cpp:50
float accX
Definition: Protocol.cpp:45
void * createSocket(void *arg)
Definition: DroneSwarm.cpp:70
static const int MSP_RAW_IMU
Definition: Protocol.h:41
float gyroX
Definition: Protocol.cpp:48
int yaw
Definition: Protocol.cpp:42
void Callback(const edrone_client::edrone_msgs::ConstPtr &msg)
Definition: DroneSwarm.cpp:141


edrone_client
Author(s): Simranjeet Singh
autogenerated on Sun Dec 1 2019 03:30:51