first commit

This commit is contained in:
senn 2020-05-18 10:46:35 +02:00
commit ff1973d025
19 changed files with 1917 additions and 0 deletions

237
CMakeLists.txt Normal file
View file

@ -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)

0
README.md Normal file
View file

85
package.xml Normal file
View file

@ -0,0 +1,85 @@
<?xml version="1.0"?>
<package format="2">
<name>esdev</name>
<version>0.0.0</version>
<description>The esdev package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="senn@todo.todo">senn</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/esdev</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>ncurses++</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<build_export_depend>move_base_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>move_base_msgs</exec_depend>
<exec_depend>ncurses</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<depend>libncurses-dev</depend>
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

87
src/cmd_vel_gate.cpp Normal file
View file

@ -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 <iostream>
//#include <stdio.h>
//#include <stdlib.h>
//#include <string.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Bool.h>
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<geometry_msgs::Twist>(
"/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;
}

63
src/consumer.cpp Normal file
View file

@ -0,0 +1,63 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <time.h>
#include <chrono>
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<double> 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

30
src/consumer_v1.cpp Normal file
View file

@ -0,0 +1,30 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
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

57
src/consumer_v2_time.cpp Normal file
View file

@ -0,0 +1,57 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <time.h>
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

View file

@ -0,0 +1,63 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <time.h>
#include <chrono>
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<double> 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

54
src/key_polling.cpp Normal file
View file

@ -0,0 +1,54 @@
#include <iostream>
#include <ncurses.h>
#include <unistd.h>
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();
}

79
src/key_polling2.cpp Normal file
View file

@ -0,0 +1,79 @@
#define _XOPEN_SOURCE 700
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
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;
}

132
src/pos_to_cmd_vel.cpp Normal file
View file

@ -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 <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
//#include <turtlesim/Pose.h>
#include <std_msgs/Bool.h>
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<geometry_msgs::Twist>(
"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;
}

128
src/pos_to_cmd_vel_plus.cpp Normal file
View file

@ -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 <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <turtlesim/Pose.h>
#include <std_msgs/Bool.h>
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<geometry_msgs::Twist>(
"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;
}

102
src/producer.cpp Normal file
View file

@ -0,0 +1,102 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float64MultiArray.h"
#include <iostream>
#include <chrono>
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<std_msgs::Float64MultiArray>("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 <float> (rand()) / (static_cast <float> (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<double> 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<std::chrono::milliseconds> (s);
ms += std::chrono::milliseconds(2500); // 2500 millisecond
s = std::chrono::duration_cast<std::chrono::seconds> (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<std::chrono::seconds>(finish - start).count();
// double time_taken = chrono::duration_cast<std::chrono::nanoseconds>(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()
*/

47
src/producer_v1.cpp Normal file
View file

@ -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<std_msgs::Int32>("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()
*/

57
src/producer_v2_time.cpp Normal file
View file

@ -0,0 +1,57 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <time.h>
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<std_msgs::Int32>("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()
*/

View file

@ -0,0 +1,92 @@
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <iostream>
#include <chrono>
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<std_msgs::Int32>("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<double> 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<std::chrono::milliseconds> (s);
ms += std::chrono::milliseconds(2500); // 2500 millisecond
s = std::chrono::duration_cast<std::chrono::seconds> (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<std::chrono::seconds>(finish - start).count();
// double time_taken = chrono::duration_cast<std::chrono::nanoseconds>(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()
*/

265
src/send_goal.cpp Normal file
View file

@ -0,0 +1,265 @@
#include <iostream>
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
//#include <turtlesim/Pose.h>
#include <std_msgs/Bool.h>
using namespace std;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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<geometry_msgs::Twist>("/RosAria/cmd_vel", 1);
//Publish a disabling out to send to velocity command gate
ros::Publisher pub = nh.advertise<std_msgs::Bool>("/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;
}

243
src/send_goal_cpp_3.cpp Normal file
View file

@ -0,0 +1,243 @@
#include <iostream>
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <turtlesim/Pose.h>
#include <std_msgs/Bool.h>
using namespace std;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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<geometry_msgs::Twist>("/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;
}

96
src/sonar_prox_detect.cpp Normal file
View file

@ -0,0 +1,96 @@
#include<ros/ros.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/PointCloud.h>
#include<sensor_msgs/PointCloud2.h>
#include<std_msgs/Float64MultiArray.h>
#include<std_msgs/Bool.h>
#include<iostream>
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<std_msgs::Bool>("/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
}