From ff1973d0256584b07931dc4ad94f8244e6569715 Mon Sep 17 00:00:00 2001 From: senn Date: Mon, 18 May 2020 10:46:35 +0200 Subject: [PATCH] first commit --- CMakeLists.txt | 237 ++++++++++++++++++++++++++++++++ README.md | 0 package.xml | 85 ++++++++++++ src/cmd_vel_gate.cpp | 87 ++++++++++++ src/consumer.cpp | 63 +++++++++ src/consumer_v1.cpp | 30 ++++ src/consumer_v2_time.cpp | 57 ++++++++ src/consumer_v3_chrono.cpp | 63 +++++++++ src/key_polling.cpp | 54 ++++++++ src/key_polling2.cpp | 79 +++++++++++ src/pos_to_cmd_vel.cpp | 132 ++++++++++++++++++ src/pos_to_cmd_vel_plus.cpp | 128 +++++++++++++++++ src/producer.cpp | 102 ++++++++++++++ src/producer_v1.cpp | 47 +++++++ src/producer_v2_time.cpp | 57 ++++++++ src/producer_v3_chrono.cpp | 92 +++++++++++++ src/send_goal.cpp | 265 ++++++++++++++++++++++++++++++++++++ src/send_goal_cpp_3.cpp | 243 +++++++++++++++++++++++++++++++++ src/sonar_prox_detect.cpp | 96 +++++++++++++ 19 files changed, 1917 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 package.xml create mode 100644 src/cmd_vel_gate.cpp create mode 100644 src/consumer.cpp create mode 100644 src/consumer_v1.cpp create mode 100644 src/consumer_v2_time.cpp create mode 100644 src/consumer_v3_chrono.cpp create mode 100644 src/key_polling.cpp create mode 100644 src/key_polling2.cpp create mode 100644 src/pos_to_cmd_vel.cpp create mode 100644 src/pos_to_cmd_vel_plus.cpp create mode 100644 src/producer.cpp create mode 100644 src/producer_v1.cpp create mode 100644 src/producer_v2_time.cpp create mode 100644 src/producer_v3_chrono.cpp create mode 100644 src/send_goal.cpp create mode 100644 src/send_goal_cpp_3.cpp create mode 100644 src/sonar_prox_detect.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a2bed6a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,237 @@ +cmake_minimum_required(VERSION 2.8.3) +project(esdev) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-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) +find_package(catkin REQUIRED COMPONENTS + actionlib + move_base_msgs + geometry_msgs + roscpp + rospy + std_msgs + nav_msgs + message_generation + message_runtime +) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(ncurses++ REQUIRED ncurses++) +## 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 exec_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 exec_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 # Or other packages containing 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 exec_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 your 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 esdev +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/esdev.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 +add_executable(${PROJECT_NAME}_key_polling src/key_polling.cpp) + +## 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}) + +target_link_libraries(${PROJECT_NAME}_key_polling ${catkin_LIBRARIES} ncurses) +target_link_libraries(${PROJECT_NAME}_key_polling ${ncurses++_LIBRARIES}) + +add_executable(${PROJECT_NAME}_key_polling2 src/key_polling2.cpp) +target_link_libraries(${PROJECT_NAME}_key_polling2 ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_sonar_prox_detect src/sonar_prox_detect.cpp) +target_link_libraries(${PROJECT_NAME}_sonar_prox_detect ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_pos_to_cmd_vel src/pos_to_cmd_vel.cpp) +target_link_libraries(${PROJECT_NAME}_pos_to_cmd_vel ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_cmd_vel_gate src/cmd_vel_gate.cpp) +target_link_libraries(${PROJECT_NAME}_cmd_vel_gate ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_send_goal src/send_goal.cpp) +target_link_libraries(${PROJECT_NAME}_send_goal ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_producer src/producer.cpp) +target_link_libraries(${PROJECT_NAME}_producer ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_consumer src/consumer.cpp) +target_link_libraries(${PROJECT_NAME}_consumer ${catkin_LIBRARIES}) + + +## 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 for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_esdev.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/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f4a4ad6 --- /dev/null +++ b/package.xml @@ -0,0 +1,85 @@ + + + esdev + 0.0.0 + The esdev package + + + + + senn + + + + + + TODO + + + + + + + + + + + + + + catkin + geometry_msgs + actionlib + actionlib_msgs + move_base_msgs + roscpp + ncurses++ + rospy + std_msgs + geometry_msgs + actionlib + actionlib_msgs + move_base_msgs + roscpp + rospy + std_msgs + geometry_msgs + actionlib + actionlib_msgs + move_base_msgs + ncurses + roscpp + rospy + std_msgs + + + + + libncurses-dev + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cmd_vel_gate.cpp b/src/cmd_vel_gate.cpp new file mode 100644 index 0000000..688f3b2 --- /dev/null +++ b/src/cmd_vel_gate.cpp @@ -0,0 +1,87 @@ +// This program subscribes to /cmd_vel_gate/cmd_vel and +// to /cmd_vel_gate/disable (Boolean) +// It republishes the cmd_vel input on /cmd_vel_gate/cmd_vel_out, +// as long as the disable input is false. +// When disable message is true, the cmd_vel output is forced to 0 +// until a disable is false message is received. + +#include +//#include +//#include +//#include +#include +#include +#include + +using namespace std; + +//global variables +ros::Publisher *pubPtr; +// Create Twist message +geometry_msgs::Twist msgOut; +bool StopFlag; + +//******************************************************* +//callback for receiving the proximity alert boolean +//generally any boolean ros message to disable the +//output while true +//this callback simply set the StopFlag global variable +//which is read by the targetPosition +void DisableReceived( + const std_msgs::Bool& ProxAlert_recv +) { + StopFlag= ProxAlert_recv.data; +} + + +void commandVelocityReceived( + const geometry_msgs::Twist& msgIn +) { +if (StopFlag == false){geometry_msgs::Twist msgOut; + msgOut.linear.x = msgIn.linear.x;//only this + msgOut.linear.y = msgIn.linear.y; + msgOut.linear.z = msgIn.linear.z; + msgOut.angular.x = msgIn.angular.x; + msgOut.angular.y = msgIn.angular.y; + msgOut.angular.z = msgIn.angular.z;//and this are used for a differential drive bot + pubPtr->publish(msgOut);} +else { + ROS_INFO("CMD VELOCITY GATED"); + cout << "Gating velocity commands" << endl; + msgOut.linear.x = 0; + msgOut.linear.y = 0; + msgOut.linear.z = 0; + msgOut.angular.x = 0; + msgOut.angular.y = 0; + msgOut.angular.z = 0; + pubPtr->publish(msgOut);} +} + + + + +//*********************************************************** +// MAIN +//*********************************************************** +int main(int argc, char **argv) { + ros::init(argc, argv, "cmd_vel_gate"); + ros::NodeHandle nh; + + //listen to the "disable" input (type boolean) + //callback when the input received a message + ros::Subscriber sub3 = nh.subscribe("/cmd_vel_gate/disable", 2, &DisableReceived); + + pubPtr = new ros::Publisher( + nh.advertise( + "/cmd_vel_gate/cmd_vel_out", + 1000)); + + //callback to receive cmd vel + ros::Subscriber sub = nh.subscribe( + "/cmd_vel_gate/cmd_vel_in", 1000, + &commandVelocityReceived); + + ros::spin(); + + delete pubPtr; +} diff --git a/src/consumer.cpp b/src/consumer.cpp new file mode 100644 index 0000000..2d470b9 --- /dev/null +++ b/src/consumer.cpp @@ -0,0 +1,63 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include +#include +#include + +using namespace std; + + +//CALLBACK FOR SUBSCRIBER CONSUMER +//-------------------------------- +void consumerCallback(const std_msgs::Int32& msg) +{ +// Record start time +auto start = std::chrono::high_resolution_clock::now(); + +ROS_INFO("I received: [%i]", msg.data); + cout << "I received: " << msg.data << endl; + +// Record end time +auto finish = std::chrono::high_resolution_clock::now(); + +std::chrono::duration elapsed = finish - start; +std::cout << "Elapsed time: " << elapsed.count() << setprecision(10) << " s\n"; + +}//end consumerCallback + + +//-------------------------------------- +// MAIN +//-------------------------------------- +int main(int argc, char **argv) +{ +//clock_t start, end; + + +ros::init(argc, argv, "consumer"); +ros::NodeHandle n; + +ros::Subscriber sub = n.subscribe("producer", 10, consumerCallback); + +//ros::Rate loop_rate(0.5); +//while (ros::ok()) +//{ + +/* Recording the starting clock tick.*/ +//start = clock(); + +//ros::spinOnce(); +//loop_rate.sleep(); + +// Recording the end clock tick. +//end = clock(); + +// Calculating total time taken by the program. klllllllopp +//double time_taken = double(end - start) / double(1000); //CLOCKS_PER_SEC +//cout << "Time taken by program is : " << fixed << time_taken << setprecision(9); +//cout << " sec " << endl; + +//}//end while +ros::spin(); +return 0; +}//end main \ No newline at end of file diff --git a/src/consumer_v1.cpp b/src/consumer_v1.cpp new file mode 100644 index 0000000..a04aa50 --- /dev/null +++ b/src/consumer_v1.cpp @@ -0,0 +1,30 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include + +using namespace std; + + +//CALLBACK FOR SUBSCRIBER CONSUMER +//-------------------------------- +void consumerCallback(const std_msgs::Int32& msg) +{ + ROS_INFO("I received: [%i]", msg.data); + cout << "I received: " << msg.data << endl; +}//end consumerCallback + + +//-------------------------------------- +// MAIN +//-------------------------------------- +int main(int argc, char **argv) +{ +ros::init(argc, argv, "consumer"); +ros::NodeHandle n; + +ros::Subscriber sub = n.subscribe("producer", 10, consumerCallback); + +ros::spin(); + +return 0; +}//end main \ No newline at end of file diff --git a/src/consumer_v2_time.cpp b/src/consumer_v2_time.cpp new file mode 100644 index 0000000..77f78cd --- /dev/null +++ b/src/consumer_v2_time.cpp @@ -0,0 +1,57 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include +#include + +using namespace std; + + +//CALLBACK FOR SUBSCRIBER CONSUMER +//-------------------------------- +void consumerCallback(const std_msgs::Int32& msg) +{ +clock_t clock_time; +clock_time = clock(); +double cktime= clock_time / double(1000); +cout << "Callback called at clock time : " << fixed + << cktime << setprecision(9); +cout << " s" << endl; +ROS_INFO("I received: [%i]", msg.data); + cout << "I received: " << msg.data << endl; +}//end consumerCallback + + +//-------------------------------------- +// MAIN +//-------------------------------------- +int main(int argc, char **argv) +{ +//clock_t start, end; + +ros::init(argc, argv, "consumer"); +ros::NodeHandle n; + +ros::Subscriber sub = n.subscribe("producer", 10, consumerCallback); + +//ros::Rate loop_rate(0.5); +//while (ros::ok()) +//{ + +/* Recording the starting clock tick.*/ +//start = clock(); + +//ros::spinOnce(); +//loop_rate.sleep(); + +// Recording the end clock tick. +//end = clock(); + +// Calculating total time taken by the program. klllllllopp +//double time_taken = double(end - start) / double(1000); //CLOCKS_PER_SEC +//cout << "Time taken by program is : " << fixed << time_taken << setprecision(9); +//cout << " sec " << endl; + +//}//end while +ros::spin(); +return 0; +}//end main \ No newline at end of file diff --git a/src/consumer_v3_chrono.cpp b/src/consumer_v3_chrono.cpp new file mode 100644 index 0000000..2d470b9 --- /dev/null +++ b/src/consumer_v3_chrono.cpp @@ -0,0 +1,63 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include +#include +#include + +using namespace std; + + +//CALLBACK FOR SUBSCRIBER CONSUMER +//-------------------------------- +void consumerCallback(const std_msgs::Int32& msg) +{ +// Record start time +auto start = std::chrono::high_resolution_clock::now(); + +ROS_INFO("I received: [%i]", msg.data); + cout << "I received: " << msg.data << endl; + +// Record end time +auto finish = std::chrono::high_resolution_clock::now(); + +std::chrono::duration elapsed = finish - start; +std::cout << "Elapsed time: " << elapsed.count() << setprecision(10) << " s\n"; + +}//end consumerCallback + + +//-------------------------------------- +// MAIN +//-------------------------------------- +int main(int argc, char **argv) +{ +//clock_t start, end; + + +ros::init(argc, argv, "consumer"); +ros::NodeHandle n; + +ros::Subscriber sub = n.subscribe("producer", 10, consumerCallback); + +//ros::Rate loop_rate(0.5); +//while (ros::ok()) +//{ + +/* Recording the starting clock tick.*/ +//start = clock(); + +//ros::spinOnce(); +//loop_rate.sleep(); + +// Recording the end clock tick. +//end = clock(); + +// Calculating total time taken by the program. klllllllopp +//double time_taken = double(end - start) / double(1000); //CLOCKS_PER_SEC +//cout << "Time taken by program is : " << fixed << time_taken << setprecision(9); +//cout << " sec " << endl; + +//}//end while +ros::spin(); +return 0; +}//end main \ No newline at end of file diff --git a/src/key_polling.cpp b/src/key_polling.cpp new file mode 100644 index 0000000..48f4af7 --- /dev/null +++ b/src/key_polling.cpp @@ -0,0 +1,54 @@ +#include +#include +#include + +using namespace std; + + +#define DELAY 100000 + +int main(int argc, char *argv[]) { +int x = 0, y = 0; + int max_y = 0, max_x = 0; + int next_x = 0; + int direction = 1; + + initscr(); + noecho(); + cbreak(); + curs_set(FALSE); + nodelay(stdscr, TRUE); + + // Global var `stdscr` is created by the call to `initscr()` +cout << "START IN 3 SECONDS" << endl; + +sleep(3); + +int ch = 88; +int key_pressed; + + while(1) { + getmaxyx(stdscr, max_y, max_x); + clear(); + mvprintw(y, x, "%c = %i", ch, ch); + // mvprintw(y, x, "o"); + refresh(); + + key_pressed=getch(); + +if (key_pressed != -1){ch = key_pressed;} + + + usleep(DELAY); + + next_x = x + direction; + + if (next_x >= max_x || next_x < 0) { + direction*= -1; + } else { + x+= direction; + } + } + + endwin(); +} \ No newline at end of file diff --git a/src/key_polling2.cpp b/src/key_polling2.cpp new file mode 100644 index 0000000..a5cfae6 --- /dev/null +++ b/src/key_polling2.cpp @@ -0,0 +1,79 @@ + +#define _XOPEN_SOURCE 700 +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +/* a essayer ... +struct termios argin, argout; +unsigned char ch = 0; + +int kbhit() { + tcgetattr(0,&argin); + argout = argin; + argout.c_lflag &= ~(ICANON); + argout.c_iflag &= ~(ICRNL); + argout.c_oflag &= ~(OPOST); + argout.c_cc[VMIN] = 1; + argout.c_cc[VTIME] = 0; + tcsetattr(0,TCSADRAIN,&argout); + read(0, &ch, 1); + tcsetattr(0,TCSADRAIN,&argin); +} +*/ + +int getkey() { + int character; + struct termios orig_term_attr; + struct termios new_term_attr; + + /* set the terminal to raw mode */ + tcgetattr(fileno(stdin), &orig_term_attr); + memcpy(&new_term_attr, &orig_term_attr, sizeof(struct termios)); + new_term_attr.c_lflag &= ~(ECHO|ICANON); + new_term_attr.c_cc[VTIME] = 0; + new_term_attr.c_cc[VMIN] = 0; + tcsetattr(fileno(stdin), TCSANOW, &new_term_attr); + + /* read a character from the stdin stream without blocking */ + /* returns EOF (-1) if no character is available */ + character = fgetc(stdin); + + /* restore the original terminal attributes */ + tcsetattr(fileno(stdin), TCSANOW, &orig_term_attr); + + return character; +} + + +int main() +{ + int key; + + /* initialize the random number generator */ + srand(time(NULL)); + + for (;;) { + sleep(1); + key = getkey(); + cout << "key:" << key; + /* terminate loop on ESC (0x1B) or Ctrl-D (0x04) on STDIN */ + if (key == 0x1B || key == 0x04) { + break; + } + else { + /* print random ASCII character between 0x20 - 0x7F */ + key = (rand() % 0x7F); + printf("%c", ((key < 0x20) ? (key + 0x20) : key)); + } + } + + return 0; +} + diff --git a/src/pos_to_cmd_vel.cpp b/src/pos_to_cmd_vel.cpp new file mode 100644 index 0000000..f4f75a3 --- /dev/null +++ b/src/pos_to_cmd_vel.cpp @@ -0,0 +1,132 @@ +//******************************************************* +// This program subscribes to cv_example/pos_cmd +// the position of the target from the opencv node and +// republishes on turtle1/cmd_vel, +// to steer the bot towards the target +// It also subscribes to turtle1/pose wich is the position +// the bot send, but don't do anything with that in the end. +// It also listen to ProxAlert messages which allow to +// disable sending of velocity commands when true. +//******************************************************* + +#include +#include +#include +//#include +#include + +ros::Publisher *pubPtr; + +//float botPOSX; +//float botPOSY; +bool StopFlag; + +//******************************************************* +//callback for dealing with the target position +//read the target position on esdev_ocv/pos_cmd +//compute the steering command to send to the bot +//on topic pos_to_cmd/vel_cmd +//to be remap depending on the bot +void targetPositionReceived( + const geometry_msgs::Point& msgIn +) { + geometry_msgs::Twist msgOut; + int targetpositionX = msgIn.x; + int targetpositionY = msgIn.y; + int target_present = msgIn.z; + +if(!StopFlag)//if StopFlag is 0 go on +{ + if (target_present == 1) + {//target here : turn and advance towards it + + if ((targetpositionX >= 310) && (targetpositionX <= 330))//if 310 < x < 330 go stright line + { + msgOut.linear.x = 0.3; + msgOut.angular.z = 0.0; + } + else if (targetpositionX > 330)//if x > 330 turn right + { + msgOut.linear.x = 0.3; + msgOut.angular.z = -0.7*(targetpositionX-320)/320;//turn right faster if target is far on the rigth + } + else if (targetpositionX < 310)//if x< 330 turn left + { + msgOut.linear.x = 0.3; + msgOut.angular.z = -0.7*(targetpositionX-320)/320;//turn left faster if target is far on the left + } + + pubPtr->publish(msgOut); + } else {//target not here : don't move + msgOut.linear.x = 0; + msgOut.angular.z = 0; + pubPtr->publish(msgOut); + } + +} +else //if StopFlag = 1 STOP the bot ! +{ + msgOut.linear.x = 0; + msgOut.angular.z = 0; + pubPtr->publish(msgOut); +} + +} + + +//******************************************************* +//callback for receiving position from the bot +//simply pass the two x,y values to two global variables +//the bot position could be used in the velocity command +//calculation +/*void poseReceived( + const turtlesim::Pose& poseIn +) { + botPOSX=poseIn.x; + botPOSY=poseIn.y; +}*/ + +//******************************************************* +//callback for receiving the proximity alert boolean +//generally any boolean ros message to disable the +//output while true +//this callback simply set the StopFlag global variable +//which is read by the targetPosition +void ProxAlertReceived( + const std_msgs::Bool& ProxAlert_recv +) { + StopFlag= ProxAlert_recv.data; +} + + + +//******************************************************* +//MAIN ************************************************** +//******************************************************* +int main(int argc, char **argv) { + ros::init(argc, argv, "pos_to_vel_cmd"); + ros::NodeHandle nh; + + //callback to listen to target position and to treat it + ros::Subscriber sub1 = nh.subscribe( + "esdev_ocv/pos_cmd", 2, + &targetPositionReceived); + + //callback to publish computer velocity command to the bot + pubPtr = new ros::Publisher( + nh.advertise( + "pos_to_cmd/cmd_vel",//turtle1/cmd_vel for turtlesim bot, or remap in a launch file + 1000)); + + /*ros::Subscriber sub2 = nh.subscribe( + "turtle1/pose", 1000, + &poseReceived);*/ + + //callback to listen to the disable boolean input + ros::Subscriber sub3 = nh.subscribe("sonar_prox_detect/ProxAlert", 2, &ProxAlertReceived); + + ros::spin(); + + delete pubPtr; +} + diff --git a/src/pos_to_cmd_vel_plus.cpp b/src/pos_to_cmd_vel_plus.cpp new file mode 100644 index 0000000..5ad4224 --- /dev/null +++ b/src/pos_to_cmd_vel_plus.cpp @@ -0,0 +1,128 @@ +//******************************************************* +// This program subscribes to cv_example/pos_cmd +// the position of the target from the opencv node and +// republishes on turtle1/cmd_vel, +// to steer the bot towards the target +// It also subscribes to turtle1/pose wich is the position +// the bot send, but don't do anything with that in the end. +// It also listen to ProxAlert messages which allow to +// disable sending of velocity commands when true. +//******************************************************* + +#include +#include +#include +#include +#include + +ros::Publisher *pubPtr; + +float botPOSX; +float botPOSY; +bool StopFlag; + +//******************************************************* +//callback for dealing with the target position +//read the target position on cv_example/pos_cmd +//compute the steering command to send to the bot +//on topic pos_to_cmd/vel_cmd +//to be remap depending on the bot +void targetPositionReceived( + const geometry_msgs::Point& msgIn +) { + geometry_msgs::Twist msgOut; + int targetpositionX = msgIn.x; + int targetpositionY = msgIn.y; + int target_present = msgIn.z; + +if(!StopFlag)//if StopFlag is 0 go on +{ + if (target_present == 1) + {//target here : turn and advance towards it + + if ((targetpositionX >= 310) && (targetpositionX <= 330))//if 310 < x < 330 go stright line + { + msgOut.linear.x = 0.3; + msgOut.angular.z = 0.0; + } + else if (targetpositionX > 330)//if x > 330 turn right + { + msgOut.linear.x = 0.3; + msgOut.angular.z = -0.7*(targetpositionX-320)/320;//turn right faster if target is far on the rigth + } + else if (targetpositionX < 310)//if x< 330 turn left + { + msgOut.linear.x = 0.3; + msgOut.angular.z = -0.7*(targetpositionX-320)/320;//turn left faster if target is far on the left + } + + pubPtr->publish(msgOut); + } else {//target not here : don't move + msgOut.linear.x = 0; + msgOut.angular.z = 0; + pubPtr->publish(msgOut); + } + +} +else //if StopFlag = 1 STOP the bot ! +{ + msgOut.linear.x = 0; + msgOut.angular.z = 0; + pubPtr->publish(msgOut); +} + +} + + +//******************************************************* +//callback for receiving position from the bot +//simply pass the two x,y values to two global variables +//the bot position could be used in the velocity command +//calculation +void poseReceived( + const turtlesim::Pose& poseIn +) { + botPOSX=poseIn.x; + botPOSY=poseIn.y; +} + +//******************************************************* +//callback for receiving the proximity alert boolean +//simply set the StopFlag global variable +//which is read by the targetPosition +void ProxAlertReceived( + const std_msgs::Bool& ProxAlert_recv +) { + StopFlag= ProxAlert_recv.data; +} + + + +//******************************************************* +//MAIN ************************************************** +//******************************************************* +int main(int argc, char **argv) { + ros::init(argc, argv, "pos_to_vel_cmd"); + ros::NodeHandle nh; + + + pubPtr = new ros::Publisher( + nh.advertise( + "pos_to_cmd/cmd_vel",//turtle1/cmd_vel for turtlesim bot, or remap in a launch file + 1000)); + + ros::Subscriber sub1 = nh.subscribe( + "cv_example/pos_cmd", 2, + &targetPositionReceived); + + ros::Subscriber sub2 = nh.subscribe( + "turtle1/pose", 1000, + &poseReceived); + + ros::Subscriber sub3 = nh.subscribe("robot_control/ProxAlert", 2, &ProxAlertReceived); + + ros::spin(); + + delete pubPtr; +} + diff --git a/src/producer.cpp b/src/producer.cpp new file mode 100644 index 0000000..370d97e --- /dev/null +++ b/src/producer.cpp @@ -0,0 +1,102 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include "std_msgs/Float64MultiArray.h" +#include +#include + +using namespace std; + +int main(int argc, char **argv) +{ +//clock_t clock_time; +cout << "GO" << endl; +ros::init(argc, argv, "producer"); +ros::NodeHandle n; + +ros::Publisher producer_pub = n.advertise("producer", 10); + +ros::Rate loop_rate(1); + +std_msgs::Int32 msg; + + std_msgs::Float64MultiArray array_msg; + size_t count = 6400; + array_msg.data.resize(count); + for (size_t i = 0; i < count; i++) + { + float r2 = static_cast (rand()) / (static_cast (RAND_MAX/3.40282e+038));//maximum float value = 3.40282e+038 + array_msg.data[i]= r2; + } + +int counter = 0; + +while (ros::ok()) + { +//msg.data = counter; + +//chrono::system_clock::time_point time_point; +//time_point = chrono::system_clock::now(); +//std::time_t now_c = std::chrono::system_clock::to_time_t(time_point); +// cout << "Time is : " << now_c << setprecision(9); +// cout << " sec" << endl; +// cout << "Now sending : " << counter << endl; + + + +// Record start time +auto start = std::chrono::high_resolution_clock::now(); + +producer_pub.publish(array_msg); +ROS_INFO("Sending message: [%i]", counter); +ros::spinOnce(); +loop_rate.sleep(); +counter++; + +// Record end time +auto finish = std::chrono::high_resolution_clock::now(); + +std::chrono::duration elapsed = finish - start; +std::cout << "Elapsed time: " << elapsed.count() << setprecision(10) << " s\n"; + +/* +std::chrono::seconds s (1); // 1 second +std::chrono::milliseconds ms = std::chrono::duration_cast (s); + +ms += std::chrono::milliseconds(2500); // 2500 millisecond + +s = std::chrono::duration_cast (ms); // truncated + + std::cout << "ms: " << ms.count() << std::endl; + std::cout << "s: " << s.count() << std::endl; +*/ + +// Calculating total time taken by the program. + //cout << chrono::duration_cast(finish - start).count(); +// double time_taken = chrono::duration_cast(finish - start).count(); +// time_taken *= 1e-9; + +// cout << "Time taken by program is : " << fixed +// << time_taken << setprecision(9); +// cout << " sec" << endl; + + }//end while + +return 0; +}//end main + +/* EQUIVALENT PYTHON PROGRAM ********************************* +import rospy +from std_msgs.msg import Int32 +if __name__ == '__main__': + rospy.init_node("counter_publisher") + rate = rospy.Rate(5) + pub = rospy.Publisher("/counter", Int32, queue_size=10) + counter = 0 + rospy.loginfo("Publisher has been started.") + while not rospy.is_shutdown(): + counter += 1 + msg = Int32() + msg.data = counter + pub.publish(counter) + rate.sleep() +*/ \ No newline at end of file diff --git a/src/producer_v1.cpp b/src/producer_v1.cpp new file mode 100644 index 0000000..9a2cde5 --- /dev/null +++ b/src/producer_v1.cpp @@ -0,0 +1,47 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" + +int main(int argc, char **argv) +{ +clock_t clock_time; + +ros::init(argc, argv, "producer"); +ros::NodeHandle n; + +ros::Publisher producer_pub = n.advertise("producer", 1); + +ros::Rate loop_rate(0.5); + +std_msgs::Int32 msg; + +int counter = 0; + +while (ros::ok()) + { +msg.data = counter; +counter++; +producer_pub.publish(msg); + +ros::spinOnce(); +loop_rate.sleep(); + }//end while + +return 0; +}//end main + +/* EQUIVALENT PYTHON PROGRAM ********************************* +import rospy +from std_msgs.msg import Int32 +if __name__ == '__main__': + rospy.init_node("counter_publisher") + rate = rospy.Rate(5) + pub = rospy.Publisher("/counter", Int32, queue_size=10) + counter = 0 + rospy.loginfo("Publisher has been started.") + while not rospy.is_shutdown(): + counter += 1 + msg = Int32() + msg.data = counter + pub.publish(counter) + rate.sleep() +*/ \ No newline at end of file diff --git a/src/producer_v2_time.cpp b/src/producer_v2_time.cpp new file mode 100644 index 0000000..ca990a7 --- /dev/null +++ b/src/producer_v2_time.cpp @@ -0,0 +1,57 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include +#include + +using namespace std; + +int main(int argc, char **argv) +{ +clock_t clock_time; +cout << "GO" << endl; +ros::init(argc, argv, "producer"); +ros::NodeHandle n; + +ros::Publisher producer_pub = n.advertise("producer", 10); + +ros::Rate loop_rate(0.5); + +std_msgs::Int32 msg; + +int counter = 0; + +while (ros::ok()) + { +msg.data = counter; +clock_time = clock(); +double cktime= clock_time / double(1000); +cout << "New clock time is : " << fixed + << cktime << setprecision(9); +cout << " s" << endl; +cout << "Now sending : " << counter << endl; + +producer_pub.publish(msg); +ros::spinOnce(); +loop_rate.sleep(); +counter++; + }//end while + +return 0; +}//end main + +/* EQUIVALENT PYTHON PROGRAM ********************************* +import rospy +from std_msgs.msg import Int32 +if __name__ == '__main__': + rospy.init_node("counter_publisher") + rate = rospy.Rate(5) + pub = rospy.Publisher("/counter", Int32, queue_size=10) + counter = 0 + rospy.loginfo("Publisher has been started.") + while not rospy.is_shutdown(): + counter += 1 + msg = Int32() + msg.data = counter + pub.publish(counter) + rate.sleep() +*/ \ No newline at end of file diff --git a/src/producer_v3_chrono.cpp b/src/producer_v3_chrono.cpp new file mode 100644 index 0000000..6455ac7 --- /dev/null +++ b/src/producer_v3_chrono.cpp @@ -0,0 +1,92 @@ +#include "ros/ros.h" +#include "std_msgs/Int32.h" +#include +#include + +using namespace std; + +int main(int argc, char **argv) +{ +//clock_t clock_time; +cout << "GO" << endl; +ros::init(argc, argv, "producer"); +ros::NodeHandle n; + +ros::Publisher producer_pub = n.advertise("producer", 10); + +ros::Rate loop_rate(0.5); + +std_msgs::Int32 msg; + +int counter = 0; + +while (ros::ok()) + { +msg.data = counter; + +//chrono::system_clock::time_point time_point; +//time_point = chrono::system_clock::now(); +//std::time_t now_c = std::chrono::system_clock::to_time_t(time_point); +// cout << "Time is : " << now_c << setprecision(9); +// cout << " sec" << endl; +// cout << "Now sending : " << counter << endl; + + + +// Record start time +auto start = std::chrono::high_resolution_clock::now(); + +producer_pub.publish(msg); +ROS_INFO("Sending: [%i]", msg.data); +ros::spinOnce(); +loop_rate.sleep(); +counter++; + +// Record end time +auto finish = std::chrono::high_resolution_clock::now(); + +std::chrono::duration elapsed = finish - start; +std::cout << "Elapsed time: " << elapsed.count() << setprecision(10) << " s\n"; + +/* +std::chrono::seconds s (1); // 1 second +std::chrono::milliseconds ms = std::chrono::duration_cast (s); + +ms += std::chrono::milliseconds(2500); // 2500 millisecond + +s = std::chrono::duration_cast (ms); // truncated + + std::cout << "ms: " << ms.count() << std::endl; + std::cout << "s: " << s.count() << std::endl; +*/ + +// Calculating total time taken by the program. + //cout << chrono::duration_cast(finish - start).count(); +// double time_taken = chrono::duration_cast(finish - start).count(); +// time_taken *= 1e-9; + +// cout << "Time taken by program is : " << fixed +// << time_taken << setprecision(9); +// cout << " sec" << endl; + + }//end while + +return 0; +}//end main + +/* EQUIVALENT PYTHON PROGRAM ********************************* +import rospy +from std_msgs.msg import Int32 +if __name__ == '__main__': + rospy.init_node("counter_publisher") + rate = rospy.Rate(5) + pub = rospy.Publisher("/counter", Int32, queue_size=10) + counter = 0 + rospy.loginfo("Publisher has been started.") + while not rospy.is_shutdown(): + counter += 1 + msg = Int32() + msg.data = counter + pub.publish(counter) + rate.sleep() +*/ \ No newline at end of file diff --git a/src/send_goal.cpp b/src/send_goal.cpp new file mode 100644 index 0000000..22e2d2b --- /dev/null +++ b/src/send_goal.cpp @@ -0,0 +1,265 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + +using namespace std; + +typedef actionlib::SimpleActionClient MoveBaseClient; + + +//this function to read a character from the keyboard if one +//has been typed. non blocking like getchar ... stuff +//************************************************************** +int getkey() { + int character; + struct termios orig_term_attr; + struct termios new_term_attr; + + /* set the terminal to raw mode */ + tcgetattr(fileno(stdin), &orig_term_attr); + memcpy(&new_term_attr, &orig_term_attr, sizeof(struct termios)); + new_term_attr.c_lflag &= ~(ECHO|ICANON); + new_term_attr.c_cc[VTIME] = 0; + new_term_attr.c_cc[VMIN] = 0; + tcsetattr(fileno(stdin), TCSANOW, &new_term_attr); + + /* read a character from the stdin stream without blocking */ + /* returns EOF (-1) if no character is available */ + character = fgetc(stdin); + + /* restore the original terminal attributes */ + tcsetattr(fileno(stdin), TCSANOW, &orig_term_attr); + + return character; +} +//************************************************************ + + + + +//************************************************************ +// MAIN +//************************************************************ +int main(int argc, char** argv){ + +cout << "******************* SEND GOALS TO THE ROBOT ***********************" << endl; +cout << "* This application allows to send different goals (one at a time) *" << endl; +cout << "* to the naviguation stack in the form of a 'MoveBaseGoal' *" << endl; +cout << "* message published on the /move_base/goal topics *" << endl; +cout << "* to which the move_base node subscribes. *" << endl; +cout << "* Goals are : the x,y coordinates of a point in the map *" << endl; +cout << "* and an orientation for the robot. *" << endl; +cout << "*******************************************************************" << endl << endl; + + + ros::init(argc, argv, "send_goal"); + ros::NodeHandle nh; + + // Init cmd_vel publisher + //ros::Publisher pub = nh.advertise("/RosAria/cmd_vel", 1); + + //Publish a disabling out to send to velocity command gate + ros::Publisher pub = nh.advertise("/send_goal/disable",1000); + + + // Create Twist message + //geometry_msgs::Twist twist; + + MoveBaseClient client("move_base", true); + + ROS_INFO("Waiting for the action server to start"); + client.waitForServer(); + + ROS_INFO("Action server started"); + move_base_msgs::MoveBaseGoal goal; + +std_msgs::Bool disable; + +bool keyok = false; +int c; + +while(ros::ok()){ + +disable.data = false; + +cout << endl << ">> Choose your goal on the map :" << endl; +cout << ">> -----------------------------" << endl; +cout << ">> Goal A (press A) " << endl; +cout << ">> Goal B (press B) " << endl; +cout << ">> Goal C (press C) " << endl; +cout << ">> Exit (press X) " << endl; +cout << ">> "; + +c=getchar(); +getchar(); + +//cout << ">> you typed :" << c << endl;//for debug purpose + + +//POINT A +if (c == 65) { + keyok = true; + // set position + goal.target_pose.pose.position.x = -1.09; + goal.target_pose.pose.position.y = -0.033; + goal.target_pose.pose.position.z = 0.0; + + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.9946; + goal.target_pose.pose.orientation.w = 0.0207; +} +else if (c == 66){ +//POINTB + keyok = true; + // set position + goal.target_pose.pose.position.x = -1.964; + goal.target_pose.pose.position.y = -0.01133; + goal.target_pose.pose.position.z = 0.0; + + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.9997; + goal.target_pose.pose.orientation.w = 0.0207; +} +else if (c == 67){ +//POINTC + + keyok = true; + // set position + goal.target_pose.pose.position.x = -3.87814094252; + goal.target_pose.pose.position.y = -0.381594400527; + goal.target_pose.pose.position.z = 0.0; + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.647632628074; + goal.target_pose.pose.orientation.w = 0.761952740696; +} +else if (c == 88){ +//STOP NODE +ros::shutdown(); +return 0; +} + +else { +cout << ">> not recognized key, try again" << endl; +keyok = false; +} + +//keyok true --------------------------------- +//send the goal to move_base action server +//else do nothing and get back to asking a valid key +if (keyok == true){ + +ROS_INFO("Sending the goal"); + + goal.target_pose.header.stamp = ros::Time::now(); + + // set frame + goal.target_pose.header.frame_id = "map"; + + client.sendGoal(goal); + + //ROS_INFO("Waiting for the result"); + //client.waitForResult(); +int key; +enum actionlib::SimpleClientGoalState::StateEnum job_state; /*Enumerator: + PENDING 0 + ACTIVE 1 + RECALLED 2 + REJECTED 3 + PREEMPTED 4 + ABORTED 5 + SUCCEEDED 6 + LOST 7 +*/ + +cout << "PRESS ANY KEY TO ABORT" << endl; +cout << "JOB STATUS : 0-PENDING 1-ACTIVE 2-RECALLED 3-REJECTED 4-PREEMPTED 5-ABORTED 6-SUCCEEDED 7-LOST :" << endl; + +//start polling loop--------------------------------- +while(1){ + job_state = client.getState().state_;//getting move base job status + cout << job_state << "|";//echoing job status + + if (job_state == 6) {//job has succeeded, exit loop + cout << endl << "GOAL REACHED" << endl; + ROS_INFO("Succeeded"); +break;} + + else if (job_state==0 || job_state==1){//if job is pending or active, we want to be able to stop it + key = getkey();//by pressing any key + //cout << "key:" << key;//debug only + if (key != -1) { + client.cancelAllGoals(); + cout << endl << ">>>>>!!! ABORTING !!!<<<<" << endl; + //loop for flooding the robot cmd_vel with 0 velocity messages to actually stop him + //the time for move_base action server to react (several seconds + //where the bot is still moving) + //not very nice solution but it works + while(1){//Loop to block the bot while move_base finishes cancel the goal + //and stop sending cmd_vel to the bot + + disable.data = true; + pub.publish(disable); + ros::spinOnce(); + + job_state = client.getState().state_;//getting move base job status + cout << "\r JOB STATUS :" << job_state << "|";//echoing job status + + if ((job_state != 1) && (job_state != 0)) {//exit the blocking loop when job status is no more "pending" or "active" + disable.data=false; + pub.publish(disable); + ros::spinOnce(); + cout << "ABORTED" << endl; + break;//exit blocking loop + } + + /*key = getkey(); + if (key != -1){//or exit if a key is pressed - not recommended + disable.data=false; + pub.publish(disable); + ros::spinOnce(); + cout << "CANCELLED" << endl; + break;//exit blocking loop + }*/ + + usleep(50000); + }//end blocking loop +break;//exit polling loop because a key was pressed and we have ensured the goal is finished and robot stopped + }//end if key != 1 ==> a key was pressed + + else {//no key pressed, so let move base continue driving the bot to its goal + //cout<< "O:";//debug + usleep(500000);} + + }//end if job pending or active + + else{//job status is not pending or active or succeeded so move base has failed + cout << endl << "Move Base failed reaching the goal" << endl; + ROS_INFO("Failed"); + break;} + +}//end polling loop------------------------------------ + +}//end if keyok TRUE ------------------------- + + +}//end while + + return 0; +} diff --git a/src/send_goal_cpp_3.cpp b/src/send_goal_cpp_3.cpp new file mode 100644 index 0000000..c2af34b --- /dev/null +++ b/src/send_goal_cpp_3.cpp @@ -0,0 +1,243 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +typedef actionlib::SimpleActionClient MoveBaseClient; + +//************************************************************** +int getkey() { + int character; + struct termios orig_term_attr; + struct termios new_term_attr; + + /* set the terminal to raw mode */ + tcgetattr(fileno(stdin), &orig_term_attr); + memcpy(&new_term_attr, &orig_term_attr, sizeof(struct termios)); + new_term_attr.c_lflag &= ~(ECHO|ICANON); + new_term_attr.c_cc[VTIME] = 0; + new_term_attr.c_cc[VMIN] = 0; + tcsetattr(fileno(stdin), TCSANOW, &new_term_attr); + + /* read a character from the stdin stream without blocking */ + /* returns EOF (-1) if no character is available */ + character = fgetc(stdin); + + /* restore the original terminal attributes */ + tcsetattr(fileno(stdin), TCSANOW, &orig_term_attr); + + return character; +} +//************************************************************ + + + + +//************************************************************ +// MAIN +//************************************************************ +int main(int argc, char** argv){ + +cout << "******************* SEND GOALS TO THE ROBOT ***********************" << endl; +cout << "* This application allows to send different goals (one at a time) *" << endl; +cout << "* to the naviguation stack in the form of a 'MoveBaseGoal' *" << endl; +cout << "* message published on the /move_base/goal topics *" << endl; +cout << "* to which the move_base node subscribes. *" << endl; +cout << "* Goals are : the x,y coordinates of a point in the map *" << endl; +cout << "* and an orientation for the robot. *" << endl; +cout << "*******************************************************************" << endl << endl; + + + ros::init(argc, argv, "send_goal_cpp"); + ros::NodeHandle nh; + + // Init cmd_vel publisher + ros::Publisher pub = nh.advertise("/RosAria/cmd_vel", 1); + + // Create Twist message + geometry_msgs::Twist twist; + + MoveBaseClient client("move_base", true); + + ROS_INFO("Waiting for the action server to start"); + client.waitForServer(); + + ROS_INFO("Action server started"); + move_base_msgs::MoveBaseGoal goal; + +bool keyok = false; +int c; + +while(ros::ok()){ + +cout << endl << ">> Choose your goal on the map :" << endl; +cout << ">> -----------------------------" << endl; +cout << ">> Goal A (press A) " << endl; +cout << ">> Goal B (press B) " << endl; +cout << ">> Goal C (press C) " << endl; +cout << ">> Exit (press X) " << endl; +cout << ">> "; + +c=getchar(); +getchar(); + +//cout << ">> you typed :" << c << endl;//for debug purpose + + +//POINT A +if (c == 65) { + keyok = true; + // set position + goal.target_pose.pose.position.x = -1.09; + goal.target_pose.pose.position.y = -0.033; + goal.target_pose.pose.position.z = 0.0; + + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.9946; + goal.target_pose.pose.orientation.w = 0.0207; +} +else if (c == 66){ +//POINTB + keyok = true; + // set position + goal.target_pose.pose.position.x = -1.964; + goal.target_pose.pose.position.y = -0.01133; + goal.target_pose.pose.position.z = 0.0; + + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.9997; + goal.target_pose.pose.orientation.w = 0.0207; +} +else if (c == 67){ +//POINTC + + keyok = true; + // set position + goal.target_pose.pose.position.x = -3.87814094252; + goal.target_pose.pose.position.y = -0.381594400527; + goal.target_pose.pose.position.z = 0.0; + // set orientation + goal.target_pose.pose.orientation.x = 0.0; + goal.target_pose.pose.orientation.y = 0.0; + goal.target_pose.pose.orientation.z = 0.647632628074; + goal.target_pose.pose.orientation.w = 0.761952740696; +} +else if (c == 88){ +//STOP NODE +ros::shutdown(); +return 0; +} + +else { +cout << ">> not recognized key, try again" << endl; +keyok = false; +} + +//keyok true --------------------------------- +//send the goal to move_base action server +//else do nothing and get back to asking a valid key +if (keyok == true){ + +ROS_INFO("Sending the goal"); + + goal.target_pose.header.stamp = ros::Time::now(); + + // set frame + goal.target_pose.header.frame_id = "map"; + + client.sendGoal(goal); + + //ROS_INFO("Waiting for the result"); + //client.waitForResult(); +int key; +enum actionlib::SimpleClientGoalState::StateEnum job_state; /*Enumerator: + PENDING 0 + ACTIVE 1 + RECALLED 2 + REJECTED 3 + PREEMPTED 4 + ABORTED 5 + SUCCEEDED 6 + LOST 7 +*/ + +cout << "PRESS ANY KEY TO ABORT" << endl; +cout << "JOB STATUS : 0-PENDING 1-ACTIVE 2-RECALLED 3-REJECTED 4-PREEMPTED 5-ABORTED 6-SUCCEEDED 7-LOST :" << endl; + +//start polling loop--------------------------------- +while(1){ + job_state = client.getState().state_;//getting move base job status + cout << job_state << "|";//echoing job status + + if (job_state == 6) {//job has succeeded, exit loop + cout << endl << "GOAL REACHED" << endl; + ROS_INFO("Succeeded"); + break;} + else if (job_state==0 || job_state==1){//if job is pending or active, who want to be able to stop it + key = getkey();//by pressing any key + //cout << "key:" << key;//debug only + if (key != -1) { + client.cancelAllGoals(); + cout << endl << ">>>>>!!! ABORTED !!!<<<<" << endl; + //loop for flooding the robot cmd_vel with 0 velocity messages to actually stop him + //the time for move_base action server to react (several seconds + //where the bot is still moving) + //not very nice solution but it works + while(1){ + // Update the Twist message + twist.linear.x = 0; + twist.linear.y = 0; + twist.linear.z = 0; + + twist.angular.x = 0; + twist.angular.y = 0; + twist.angular.z = 0; + + // Publish it and resolve any remaining callbacks + pub.publish(twist); + ros::spinOnce(); + + job_state = client.getState().state_;//getting move base job status + cout << "\r JOB STATUS :" << job_state << "|";//echoing job status + + if (job_state != 1) {break;}//exit the loop when job status is no more "active" + usleep(50000); + key = getkey(); + if (key != -1){break;}//or exit if a key is pressed + } + + break;} + else {//no key pressed, so let move base drive the bot to its goal + //cout<< "O:";//debug + usleep(500000);} + } + else{//job status is not pending or active or succeeded so move base has failed + cout << endl << "Move Base failed reaching the goal" << endl; + ROS_INFO("Failed"); + break;} + +}//end polling loop------------------------------------ + +}//end if keyok TRUE ------------------------- + + +}//end while + + return 0; +} diff --git a/src/sonar_prox_detect.cpp b/src/sonar_prox_detect.cpp new file mode 100644 index 0000000..3b2ebab --- /dev/null +++ b/src/sonar_prox_detect.cpp @@ -0,0 +1,96 @@ +#include +#include +#include +#include +#include +#include +#include + + +using namespace std; + +std_msgs::Bool ProxAlert; +sensor_msgs::PointCloud sonar; + +//structure to bear sonar readings from the bot and store them +//in sonar_n member +/*data is Rosaria/sonar type which is sensor_msgs/PointCloud.msg which is: + std_msgs/Header header + geometry_msgs/Point32[] points + sensor_msgs/ChannelFloat32[] channels +*/ + +class sonar_value + { + public: + int i; + std_msgs::Float64MultiArray minDistance; + float capteur1, capteur2, capteur3, capteur4, capteur5, capteur6; + double dist_x,sonar_2,sonar_3,sonar_4,sonar_5,dist_courbe; + void sonarCallback(const sensor_msgs::PointCloud::ConstPtr& data); + //void irCallback (const robot_control::Adc::ConstPtr& valeur); + }; + +void sonar_value::sonarCallback(const sensor_msgs::PointCloud::ConstPtr& data) + { + + minDistance.data.clear(); + minDistance.data.resize(6); + + for (i=2;i<6;i++) + { + + //dist_courbe=(sqrt(pow(data->points[i].x,2)+pow(data->points[i].y,2)+pow(data->points[i].z,2))); + dist_x = data->points[i].x;//actually read the sensors values + minDistance.data[i] = dist_x;//store them in minDistance tab + } + sonar_2 = minDistance.data[2];//copy them in sonar_n value + sonar_3 = minDistance.data[3]; + sonar_4 = minDistance.data[4]; + sonar_5 = minDistance.data[5]; + //ROS_INFO("sonar 3 detecte: %f et sonar 4: %f",sonar_3, sonar_4);//for debug + } + + +int main(int argc, char **argv) +{ + ROS_INFO_STREAM("MAIN"); + ros::init(argc, argv, "robot_control"); + ros::NodeHandle nh; + sonar_value obj; + + //subscribe to sonar data from rosaria + ros::Subscriber sonar_sub = nh.subscribe("RosAria/sonar", 1000, &sonar_value::sonarCallback, &obj); + + //publish proximity alert message to whoever wants to ear + ros::Publisher ProxAlert_pub = nh.advertise("/sonar_prox_detect/ProxAlert",1000); + + double CLOCK_SPEED = 1.5; + + ros::Rate rate(CLOCK_SPEED); + + + // ros::Time start = ros::Time::now(); + while(ros::ok()) + { + //ROS_INFO_STREAM("WHILE"); + //if (obj.capteur1>4.5 && obj.capteur6>4.5) + //ROS_INFO("readings on sonar 2: %f 3: %f 4: %f 5: %f",obj.sonar_2, obj.sonar_3, obj.sonar_4, obj.sonar_5); + if ( (obj.sonar_2 < 1) || (obj.sonar_3 < 1) || (obj.sonar_4 < 1) || (obj.sonar_5 < 1) ) + { + ROS_INFO("PROXIMITY ALERT"); + ProxAlert.data = true; + ProxAlert_pub.publish(ProxAlert); + } + else + { + ProxAlert.data = false; + ProxAlert_pub.publish(ProxAlert); + } + + + ros::spinOnce(); + rate.sleep(); + }//end while + +}