Skip to content

Commit

Permalink
Merge pull request #30 from IseanB/multiDroneControl
Browse files Browse the repository at this point in the history 
Updatined dev branch
  • Loading branch information
@IseanB
IseanB authored Aug 26, 2022
2 parents c6f506a + c0b4fde commit 5a23516
Show file tree
Hide file tree
Showing 9 changed files with 280 additions and 673 deletions.
11 changes: 3 additions & 8 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,11 +154,6 @@ add_executable(single_control
helper/src/conversions.cpp
helper/src/computations.cpp
src/src/singleDrone.cpp)

# add_executable(singleDroneControl_node
# helper/src/conversions.cpp
# helper/src/computations.cpp
# src/src/single_control.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
Expand Down Expand Up @@ -234,9 +229,9 @@ target_link_libraries(single_control
#############

## Add gtest based cpp test target and link libraries
# catkin_add_executable_with_gtest(simple_mov_test
# helper/src/computations.cpp
# test/test_helperFunc.cpp)
catkin_add_executable_with_gtest(test_drone_control
helper/src/computations.cpp
test/test_helperFunc.cpp)
# if(TARGET simple_mov_test)
# target_link_libraries(simple_mov_test ${PROJECT_NAME})
# endif()
Expand Down
6 changes: 2 additions & 4 deletions helper/computations.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,14 +11,12 @@
/* Functions */

/*Makes sures this_pos is not tilted more than maxTilt, degrees, in x or y axis*/
bool isFlat(const geometry_msgs::PoseStamped& this_pos, float maxTilt = 0);
bool isFlat(const geometry_msgs::Pose& this_pos, float maxTilt = 0);

/* Finds magnitude of this_vel values, x and y and z, checks its less than or equal to maxSpeed(m/s)*/
bool isStationary(const geometry_msgs::Twist& this_vel, float maxSpeed = 0, float maxTiltSpeed = 0);

/*Finds distance from curr_pos to desired_pos and checks it's within a accuracyDistance*/
bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance);
bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance);
bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance);

bool reachedLocation(const geometry_msgs::Pose& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance);
#endif
27 changes: 10 additions & 17 deletions helper/src/computations.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,30 +12,23 @@ bool isStationary(const geometry_msgs::Twist& this_vel, float maxSpeed, float ma
);
}

bool isFlat(const geometry_msgs::PoseStamped& this_pos, float maxTilt){
return (abs(this_pos.pose.orientation.x) <= abs(maxTilt) && abs(this_pos.pose.orientation.y) <= abs(maxTilt));
bool isFlat(const geometry_msgs::Pose& this_pos, float maxTilt){
return (abs(this_pos.orientation.x) <= abs(maxTilt) && abs(this_pos.orientation.y) <= abs(maxTilt));
}

bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance){
bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::PoseStamped& desired_pos, float accuracyDistance){
return(
pow(
pow(this_pos.pose.position.x - desired_pos.pose.position.x, 2) +
pow(this_pos.pose.position.y - desired_pos.pose.position.y, 2) +
pow(this_pos.pose.position.z - desired_pos.pose.position.z, 2), .5) <= abs(accuracyDistance) );
pow(this_pos.position.x - desired_pos.pose.position.x, 2) +
pow(this_pos.position.y - desired_pos.pose.position.y, 2) +
pow(this_pos.position.z - desired_pos.pose.position.z, 2), .5) <= abs(accuracyDistance) );
}

bool reachedLocation(const geometry_msgs::Pose& this_pos, const geometry_msgs::Point& desired_pos, float accuracyDistance){
return(
pow(
pow(this_pos.position.x - desired_pos.x, 2) +
pow(this_pos.position.y - desired_pos.y, 2) +
pow(this_pos.position.z - desired_pos.z, 2), .5) <= abs(accuracyDistance) );
}

bool reachedLocation(const geometry_msgs::PoseStamped& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance){
bool reachedLocation(const geometry_msgs::Pose& this_pos, const mavros_msgs::PositionTarget& desired_pos, float accuracyDistance){
return(
pow(
pow(this_pos.pose.position.x - desired_pos.position.x, 2) +
pow(this_pos.pose.position.y - desired_pos.position.y, 2) +
pow(this_pos.pose.position.z - desired_pos.position.z, 2), .5) <= abs(accuracyDistance) );
pow(this_pos.position.x - desired_pos.position.x, 2) +
pow(this_pos.position.y - desired_pos.position.y, 2) +
pow(this_pos.position.z - desired_pos.position.z, 2), .5) <= abs(accuracyDistance) );
}
54 changes: 0 additions & 54 deletions launch/oneDrone.launch
View file

This file was deleted.

2 changes: 1 addition & 1 deletion msg/dresponse.msg
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,2 @@
# Responses: States, ERROR, RECEIVED, REACHED
# Responses: States, ERROR, RECEIVED, REACHED, NULL
std_msgs/String response
63 changes: 23 additions & 40 deletions src/src/multiDrone.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,13 @@ int main(int argc, char **argv)
ros::Publisher drone1_cmd_pub = nh.advertise<drone_control::dcontrol>
("drone0/cmds", 5);
ros::Publisher drone2_cmd_pub = nh.advertise<drone_control::dcontrol>
("drone12/cmds", 5);
("drone1/cmds", 5);
ros::Publisher drone3_cmd_pub = nh.advertise<drone_control::dcontrol>
("drone22/cmds", 5);
("drone2/cmds", 5);
ros::Publisher drone4_cmd_pub = nh.advertise<drone_control::dcontrol>
("drone32/cmds", 5);
("drone3/cmds", 5);

ros::Subscriber multi_cmd_sub = nh.subscribe<drone_control::dresponse>("drone0/info", 0, storeInfo);

drone_control::dcontrol msg;
bool lift = false;
Expand All @@ -30,8 +32,6 @@ int main(int argc, char **argv)
bool transit4 = false;
bool transit5 = false;
bool transit6 = false;
bool transit7 = false;
// bool transit2 = false;
while(ros::ok() && !lift){
if(ros::Time::now() - last_request >= ros::Duration(1.0)){
ROS_INFO("LIFT info sent");
Expand All @@ -47,7 +47,7 @@ int main(int argc, char **argv)
ros::spinOnce();
rate.sleep();
}
while(ros::ok() && !transit7){
while(ros::ok() && !transit6){
if(!transit && ros::Time::now() - last_request >= ros::Duration(15.0)){
ROS_INFO(" TRANSIT ADD");
msg.command.data = "TRANSIT_ADD";
Expand All @@ -61,7 +61,7 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
transit = true;
}
else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(10.0)){
else if(!transit2 && transit && ros::Time::now() - last_request >= ros::Duration(2.0)){
ROS_INFO(" TRANSIT ADD");
msg.command.data = "TRANSIT_ADD";
msg.target.x = -1;
Expand All @@ -74,11 +74,11 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
transit2 = true;
}
else if(!transit3 && transit2 && ros::Time::now() - last_request >= ros::Duration(1.0)){
else if(!transit3 && transit2 && ros::Time::now() - last_request >= ros::Duration(6.0)){
ROS_INFO(" TRANSIT ADD");
msg.command.data = "TRANSIT_ADD";
msg.target.x = 0;
msg.target.y = 0;
msg.target.x = 3;
msg.target.y = 3;
msg.target.z = 1;
drone1_cmd_pub.publish(msg);
drone2_cmd_pub.publish(msg);
Expand All @@ -87,11 +87,11 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
transit3 = true;
}
else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(15.0)){
ROS_INFO(" TRANSIT NEW");
else if(!transit4 && transit3 && ros::Time::now() - last_request >= ros::Duration(10.0)){
ROS_INFO(" TRANSIT_NEW");
msg.command.data = "TRANSIT_NEW";
msg.target.x = -1;
msg.target.y = 2;
msg.target.x = -2;
msg.target.y = -4;
msg.target.z = 2;
drone1_cmd_pub.publish(msg);
drone2_cmd_pub.publish(msg);
Expand All @@ -100,46 +100,29 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
transit4 = true;
}
else if(!transit5 && transit4 && ros::Time::now() - last_request >= ros::Duration(3.0)){
ROS_INFO(" TRANSIT NEW");
msg.command.data = "TRANSIT_NEW";
msg.target.x = 1;
msg.target.y = -2;
msg.target.z = 2;
else if(!transit5 && transit4 && ros::Time::now() - last_request >= ros::Duration(4.0)){
ROS_INFO(" TRANSIT_ADD");
msg.command.data = "TRANSIT_ADD";
msg.target.x = 0;
msg.target.y = 0;
msg.target.z = 3;
drone1_cmd_pub.publish(msg);
drone2_cmd_pub.publish(msg);
drone3_cmd_pub.publish(msg);
drone4_cmd_pub.publish(msg);
last_request = ros::Time::now();
transit5 = true;
}
else if(!transit6 && transit5 && ros::Time::now() - last_request >= ros::Duration(3.0)){
ROS_INFO(" TRANSIT ADD");
msg.command.data = "TRANSIT_ADD";
msg.target.x = 3;
msg.target.y = -2;
msg.target.z = 2;
else if(!transit6 && transit5 && ros::Time::now() - last_request >= ros::Duration(15.0)){
ROS_INFO(" LAND");
msg.command.data = "LAND";
drone1_cmd_pub.publish(msg);
drone2_cmd_pub.publish(msg);
drone3_cmd_pub.publish(msg);
drone4_cmd_pub.publish(msg);
last_request = ros::Time::now();
transit6 = true;
}
else if(!transit7 && transit6 && ros::Time::now() - last_request >= ros::Duration(1.0)){
ROS_INFO(" TRANSIT NEW");
msg.command.data = "TRANSIT_NEW";
msg.target.x = 0;
msg.target.y = 0;
msg.target.z = 2;
drone1_cmd_pub.publish(msg);
drone2_cmd_pub.publish(msg);
drone3_cmd_pub.publish(msg);
drone4_cmd_pub.publish(msg);
last_request = ros::Time::now();
transit7 = true;
ROS_INFO("DONE");
}
ros::spinOnce();
rate.sleep();
}
Expand Down
Loading

0 comments on commit 5a23516

Please sign in to comment.