diff --git a/Strategy/Prototype/CMakeLists.txt b/Strategy/Prototype/CMakeLists.txt new file mode 100644 index 0000000..0b887a4 --- /dev/null +++ b/Strategy/Prototype/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 2.8.3) +project(strategy) + +## Add support for C++11, supported in ROS Kinetic and newer +# add_definitions(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES strategy +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/strategy.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +set(STRATEGY + src/strategy.cpp +src/strategy.h +src/main.cpp +) + add_executable(${PROJECT_NAME}_node ${STRATEGY} ) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_strategy.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Strategy/Prototype/main.cpp b/Strategy/Prototype/main.cpp new file mode 100644 index 0000000..a6a91e5 --- /dev/null +++ b/Strategy/Prototype/main.cpp @@ -0,0 +1,28 @@ +#include "strategy.h" +#include "ros/ros.h" +#include + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "strategy"); + strategy STRATEGY; + while(ros::ok()) + { + STRATEGY.ComputeDistance(); + STRATEGY.FindBotsInsideCircle(); + STRATEGY.FirstOperation(); + + while(STRATEGY.IsOutsideWhite()) + { + time_t timer1, timer2; + timer1 = time(NULL); + STRATEGY.t_plan(); + STRATEGY.FirstOperation(); + timer2 = time(NULL) - timer1; + while(timer2<=20) + timer2 = time(NULL) - timer1; + + } + } +} diff --git a/Strategy/Prototype/strategy.cpp b/Strategy/Prototype/strategy.cpp new file mode 100644 index 0000000..9bd6b13 --- /dev/null +++ b/Strategy/Prototype/strategy.cpp @@ -0,0 +1,476 @@ +#include //confirm x and y axis from Aditi +#include +#include +#include +#include +#include "nav_msgs/Odometry.h" +#include "strategy.h" +#include "ros/ros.h" + +#define PI 3.14159 +#define MIN_DIST 9 + +using namespace std; + +int strategy::IsOutsideWhite() +{ + int centerBotID; + p temp = *ClosestBot.begin(); + centerBotID = temp.second; + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", centerBotID); + ros::Rate loop_rate(10); + sub = n.subscribe(topic_name, 1, &strategy::centercallback,this); + ros::spinOnce(); + loop_rate.sleep(); + if(centerY>=MIN_DIST) + return 1; + else + return 0; +} + +void strategy::posecallback(nav_msgs::Odometry::ConstPtr msg){ + + posX = msg->pose.pose.position.y; + posY = -msg->pose.pose.position.x; + x = msg->pose.pose.orientation.x; + y = msg->pose.pose.orientation.y; + z = msg->pose.pose.orientation.z; + w = msg->pose.pose.orientation.w; +} + +void strategy::centercallback(nav_msgs::Odometry::ConstPtr msg){ + + centerX = msg->pose.pose.position.y; + centerY = -msg->pose.pose.position.x; + x = msg->pose.pose.orientation.x; + y = msg->pose.pose.orientation.y; + z = msg->pose.pose.orientation.z; + w = msg->pose.pose.orientation.w; +} + + +void strategy::find_herd_bots() +{ + float max; + for(int i=4;i<14;i++){ + + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", i); + sub = n.subscribe(topic_name, 1, &strategy::posecallback,this); + ros::spinOnce(); + double y = posY; + distance_bots.push_back(y); + } + + max=-FLT_MAX; + no1 = no2 = 0; + for(int i=4;i<14;i++){ + if(distance_bots[i]<0){ + if(distance_bots[i]>=max){ + no2 = no1; + max=distance_bots[i]; + no1 = i; + } + } + } + herd_bots(no1, no2); +} + +void strategy::herd_bots(int no1, int no2) //go to the no1 and no2 bot and tap for turning them towards the center +{ + int bots[2] = {no1, no2}; + int z=0; + while(z<=1) + { + double yaw=0, pitch=0, roll=0; + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", bots[z]); + sub = n.subscribe(topic_name, 1000, &strategy::posecallback,this); + ros::spinOnce(); + GetEulerAngles(&yaw, &pitch, &roll); + + pub = n.advertise(topic_name, 1000); + nav_msgs::Odometry msg; + double theta1 = atan((10 - posY)/(10 - posX)); + double theta2 = atan((10 - posY)/(-10 - posX)); + + /*if(yaw>=theta2 && yaw<=theta1) + //do nothing*/ + if((yaw>=angle(theta1+PI) && yaw<=angle(theta2+PI) ) || (angle(theta1+PI)*angle(theta2+PI)<0 && (yaw>=angle(theta1+PI) || yaw<=angle(theta2+PI)) ) ) + { + //180 degree turn, come in front + yaw = angle(yaw + PI); + toQuaternion(yaw,pitch,roll); + msg.pose.pose.orientation.x=x; + msg.pose.pose.orientation.y=y; + msg.pose.pose.orientation.z=z; + msg.pose.pose.orientation.w=w; + pub.publish(msg); + } + else if((yaw>theta2 && yawtheta2 || yaw=angle(theta2 + PI/4) && yaw<=angle(theta2 + PI/2) || ( angle(theta2+PI/4)*angle(theta2+PI/2)<0 && (yaw>angle(theta1+PI/4) || yaw=angle(theta2+PI) && yaw<=angle(theta2+PI+PI/4) || (angle(theta2+PI)*angle(theta2+PI+PI/4) && (yaw>=angle(theta2+PI) && yaw<=angle(theta2+PI+PI/4)))) + { + //first rotate by 180 degrees and then turn by 45 degrees + yaw = angle(yaw-PI-PI/4); + toQuaternion(yaw,pitch,roll); + msg.pose.pose.orientation.x=x; + msg.pose.pose.orientation.y=y; + msg.pose.pose.orientation.z=z; + msg.pose.pose.orientation.w=w; + pub.publish(msg); + } + z++; + } + +} + + +void strategy::ComputeDistance(){ //computes distance of every bot from the green line + + for(int i=4;i<14;i++){ + + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", i); + sub = n.subscribe(topic_name, 1, &strategy::posecallback,this); + ros::spinOnce(); + double y = 10 - posY; + int BotID = i; + ClosestBot.insert(make_pair(y,BotID)); + + } +} + + +void strategy::FindBotsInsideCircle(){ //to find the bot inside the 5m circle + + int centerBotID; + + p temp = *ClosestBot.begin(); + centerBotID = temp.second; + + int count=4; + + while(count<14){ + + if(count!=centerBotID){ + + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", centerBotID); + sub = n.subscribe(topic_name, 1, &strategy::centercallback,this); + ros::spinOnce(); + char topic_name2[40]; + sprintf(topic_name2, "robot%d/odom", count); + sub = n.subscribe(topic_name2, 1, &strategy::posecallback,this); + ros::spinOnce(); + } + + + if((pow(posX-centerX,2) + pow(posY-centerY,2) - 25) <= 0 && count!=centerBotID){ + + BotsInsideCircle.push_back(count); + } + count++; + } +} + +void strategy::FirstOperation(){ //to decide the first operation of the center bot + + double yaw=0, pitch=0, roll=0; + int centerBotID; + + p temp = *ClosestBot.begin(); + centerBotID = temp.second; + + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", centerBotID); + sub = n.subscribe(topic_name, 1000, &strategy::centercallback,this); + ros::spinOnce(); + GetEulerAngles(&yaw, &pitch, &roll); + + pub = n.advertise(topic_name, 1000); + nav_msgs::Odometry msg; + double theta1 = atan((10 - centerY)/(10 - centerX)); + double theta2 = atan((10 - centerY)/(-10 - centerX)); + + /*if(yaw>=theta2 && yaw<=theta1) + //do nothing*/ + if((yaw>=angle(theta1+PI) && yaw<=angle(theta2+PI) ) || (angle(theta1+PI)*angle(theta2+PI)<0 && (yaw>=angle(theta1+PI) || yaw<=angle(theta2+PI)) ) ) + { + //180 degree turn, come in front + yaw = angle(yaw + PI); + toQuaternion(yaw,pitch,roll); + msg.pose.pose.orientation.x=x; + msg.pose.pose.orientation.y=y; + msg.pose.pose.orientation.z=z; + msg.pose.pose.orientation.w=w; + pub.publish(msg); + } + else if((yaw>theta2 && yawtheta2 || yaw=angle(theta2 + PI/4) && yaw<=angle(theta2 + PI/2) || ( angle(theta2+PI/4)*angle(theta2+PI/2)<0 && (yaw>angle(theta1+PI/4) || yaw=angle(theta2+PI) && yaw<=angle(theta2+PI+PI/4) || (angle(theta2+PI)*angle(theta2+PI+PI/4) && (yaw>=angle(theta2+PI) && yaw<=angle(theta2+PI+PI/4)))) + { + //first rotate by 180 degrees and then turn by 45 degrees + yaw = angle(yaw-PI-PI/4); + toQuaternion(yaw,pitch,roll); + msg.pose.pose.orientation.x=x; + msg.pose.pose.orientation.y=y; + msg.pose.pose.orientation.z=z; + msg.pose.pose.orientation.w=w; + pub.publish(msg); + } +} + + +void strategy::GetEulerAngles( double* yaw, double* pitch, double* roll) + { + const double w2 = w*w; + const double x2 = x*x; + const double y2 = y*y; + const double z2 = z*z; + const double unitLength = w2 + x2 + y2 + z2; // Normalised == 1, otherwise correction divisor. + const double abcd = w*x + y*z; + const double eps = 1e-7; // TODO: pick from your math lib instead of hardcoding. + const double pi = 3.14159265358979323846; // TODO: pick from your math lib instead of hardcoding. + if (abcd > (0.5-eps)*unitLength) + { + *yaw = 2 * atan2(y, w); + *pitch = pi; + *roll = 0; + } + else if (abcd < (-0.5+eps)*unitLength) + { + *yaw = -2 * ::atan2(y, w); + *pitch = -pi; + *roll = 0; + } + else + { + const double adbc = w*z - x*y; + const double acbd = w*y - x*z; + *yaw = ::atan2(2*adbc, 1 - 2*(z2+x2)); + *pitch = ::asin(2*abcd/unitLength); + *roll = ::atan2(2*acbd, 1 - 2*(y2+x2)); + } + + } + + +void strategy::toQuaternion(double pitch, double roll, double yaw) +{ + double t0 = cos(yaw * 0.5); + double t1 = sin(yaw * 0.5); + double t2 = cos(roll * 0.5); + double t3 = sin(roll * 0.5); + double t4 = cos(pitch * 0.5); + double t5 = sin(pitch * 0.5); + + w = t0 * t2 * t4 + t1 * t3 * t5; + x = t0 * t3 * t4 - t1 * t2 * t5; + y = t0 * t2 * t5 + t1 * t3 * t4; + z = t1 * t2 * t4 - t0 * t3 * t5; + +} + + + float strategy::dist_whitel(){ + + double yaw,pitch,roll; + GetEulerAngles(&yaw,&pitch,&roll); + float orient=yaw; + + + if(orient>=0 && orient=PI/2 && orient>PI){ + return 10+posY; + } + + if(orient<0 && orient>-PI/2){ + if(10-posX<10+posY) + return 10-posX; + else + return 10+posY; + } + + if(orient>-PI && orient<-PI/2){ + if(10+posY<10+posX) + return 10+posY; + else + return 10+posX; + } + +} + +float strategy::angle(float ang){ + + if(ang>=0 && ang<=PI) + return ang; + else if(ang>PI) + return ang-2*PI; + else if(ang<-PI) + return ang+2*PI; + +} + + +void strategy::action(int bot_no){ + float theta1,theta2,orient; + char topic_name[40]; + sprintf(topic_name, "robot%d/odom", bot_no); + + theta1=angle(atan((centerY-posY)/(centerX-posX))-PI/4); + theta2=angle(theta1 + PI/2); + + double yaw,pitch,roll; + GetEulerAngles(&yaw,&pitch,&roll); + orient=yaw; + + + pub = n.advertise(topic_name, 1); + nav_msgs::Odometry msg; + msg.pose.pose.position.x = posY; + msg.pose.pose.position.y = -posX; + + if( (orient>theta2 && orienttheta2 || orient=angle(theta1+PI) && orient<=angle(theta2+PI) ) || (angle(theta1+PI)*angle(theta2+PI)<0 && (orient>=angle(theta1+PI) || orient<=angle(theta2+PI)) ) ){ + //turn 180 degree + yaw=angle(yaw+PI); + toQuaternion(yaw,pitch,roll); + msg.pose.pose.orientation.x=x; + msg.pose.pose.orientation.y=y; + msg.pose.pose.orientation.z=z; + msg.pose.pose.orientation.w=w; + pub.publish(msg); + } + else if( (orient=angle(theta1-PI/4)) || ( theta1*angle(theta1-PI/4)<0 && (orient>angle(theta1-PI/4) || orientangle(theta2+PI/4) && orient<=angle(theta2+PI/2)) || ( angle(theta2+PI/4)*angle(theta2+PI/2)<0 && (orient>angle(theta1+PI/4) || orient=angle(theta1-PI/2)) || ( angle(theta1-PI/2)*angle(theta1-PI/4)<0 && (orient>angle(theta1-PI/2) || orient +#include +#include +#include +#include +#include "ros/ros.h" +#include "nav_msgs/Odometry.h" + +using namespace std; + +typedef struct { + int x; + int y; +}point; + +class strategy{ + + public: + void find_herd_bots(); // finds the bot to be herded in the first 20 secs + void herd_bots(int no1, int no2); // herds the bots for the first 20 secs + void ComputeDistance(); // computes distance from green line + void FindBotsInsideCircle(); // finds bot inside the circle + void FirstOperation(); // decides the operation to be performed on the target bot + float angle(float ang); // to make sure the angle is within the range + float dist_whitel(); // calculates the least dist from the white line + void action(int bot_no); // action to be performed on the bots inside the circle + void t_plan(); // formulates the plan for the bot inside the circle + void GetEulerAngles(double* yaw, double* pitch, double* roll); + void posecallback(nav_msgs::Odometry::ConstPtr msg); + void centercallback(nav_msgs::Odometry::ConstPtr msg); + void toQuaternion(double pitch, double roll, double yaw); + int IsOutsideWhite(); //check is the center bot is outside green line + ros::NodeHandle n; + ros::Publisher pub; + ros::Subscriber sub; + + private: + typedef pair p; + set

ClosestBot; // set containing bot id and dist from green line + vector BotsInsideCircle; // vector containing id of bots inside the circle + double centerX, centerY; // x and y coordinates of the center/target bot + double posX, posY; // x and y coordinates of the the other bots inside the circle + double x, y, z, w; // needed for qauternian angle to euler angle + vector distance_bots; + int no1,no2; +}; + +#endif