first commit
This commit is contained in:
commit
ff1973d025
19 changed files with 1917 additions and 0 deletions
237
CMakeLists.txt
Normal file
237
CMakeLists.txt
Normal 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
0
README.md
Normal file
85
package.xml
Normal file
85
package.xml
Normal 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
87
src/cmd_vel_gate.cpp
Normal 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
63
src/consumer.cpp
Normal 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
30
src/consumer_v1.cpp
Normal 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
57
src/consumer_v2_time.cpp
Normal 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
|
63
src/consumer_v3_chrono.cpp
Normal file
63
src/consumer_v3_chrono.cpp
Normal 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
54
src/key_polling.cpp
Normal 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
79
src/key_polling2.cpp
Normal 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
132
src/pos_to_cmd_vel.cpp
Normal 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
128
src/pos_to_cmd_vel_plus.cpp
Normal 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
102
src/producer.cpp
Normal 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
47
src/producer_v1.cpp
Normal 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
57
src/producer_v2_time.cpp
Normal 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()
|
||||
*/
|
92
src/producer_v3_chrono.cpp
Normal file
92
src/producer_v3_chrono.cpp
Normal 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
265
src/send_goal.cpp
Normal 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
243
src/send_goal_cpp_3.cpp
Normal 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
96
src/sonar_prox_detect.cpp
Normal 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
|
||||
|
||||
}
|
Loading…
Reference in a new issue