diff --git a/.gitignore b/.gitignore index e9ef97c..9c5d2f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,6 @@ software/build /*cache* - +**/__pycache__ **/*.swp **/*.swo diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash new file mode 100644 index 0000000..d81d563 --- /dev/null +++ b/scripts/assistedNavigation.bash @@ -0,0 +1,21 @@ +# this script run all the necessary setup to run the assisted navigation program using the teleop keyboard + +export ROS_HOSTNAME=192.168.0.114 +# if you use other ip for ros master then do “export ROS_MASTER_URI=http://{ROS_MASTER_IP}:11311” +export ROS_MASTER_URI=http://192.168.1.105:11311 +python3 ../src/drive_controller.py +python3 ../src/tof_handler.py +python3 ../src/twist_to_pwm.py + +#run gazebo world of your choice +roslaunch gazebo_ros shapes_world.launch + +# on a new terminal to spawn the robody entity +roslaunch robody_sim spawn.launch +roslaunch robody_sim spawndefault.launch + +rosrun gazebo spawn_model -file /home/ronggurmwp/robody_nav_ws/src/robody_sim/urdf/human.urdf -urdf -z 1 -model my_object + +rosrun gazebo spawn_model -file "/home/ronggurmwp/robody_nav_ws/src/robody_sim/urdf/human.urdf" -urdf -z 1 -model my_object + +rosrun teleop_twist_keyboard teleop_twist_keyboard.py \ No newline at end of file diff --git a/scripts/assistedNavigation_aio.bash b/scripts/assistedNavigation_aio.bash new file mode 100644 index 0000000..55a802f --- /dev/null +++ b/scripts/assistedNavigation_aio.bash @@ -0,0 +1,10 @@ +# this script run all the necessary setup to run the assisted navigation program using the teleop keyboard + +export ROS_HOSTNAME=192.168.0.114 +# if you use other ip for ros master then do “export ROS_MASTER_URI=http://{ROS_MASTER_IP}:11311” +export ROS_MASTER_URI=http://192.168.1.105:11311 +python3 ../src/assistedNavigation_aio.py +python3 ../src/twist_to_pwm.py +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +rosrun teleop_twist_keyboard teleop_twist_keyboard.py \ No newline at end of file diff --git a/software/README.md b/software/README.md index 8b13789..e69de29 100644 --- a/software/README.md +++ b/software/README.md @@ -1 +0,0 @@ - diff --git a/src/README.md b/src/README.md new file mode 100644 index 0000000..1f61cd4 --- /dev/null +++ b/src/README.md @@ -0,0 +1,89 @@ +# Assisted Navigation + +## Prerequisites +- ROS1 Melodic +## Building +1. To start building, first clone the repository using the following command, specifying the "an_ToF" branch: + +``` +git clone -b an_ToF https://github.com/Roboy/esp-wheelchair.git +``` +2. Next, navigate to the "src" folder within the repository. + +3. Copy the "assisted_navigation" package to your workspace. + +4. Build the workspace using the "catkin_make" command. + +5. Finally, source the setup bash file by running the following command: +``` +source /devel/setup.sh +``` + +With these steps, you should be ready to proceed with the build process. + +## Running +1. To execute the sensor, you can use either of the following commands: + +``` +rosrun roboy_state_estimator ir_sensor_publisher.py +``` +or + +``` +rosrun assisted_navigation ir_sensor_publisher.py +``` + +2. These commands will run the "ir_sensor_publisher.py" script, which is responsible for publishing data from the IR sensor. + +3. To launch the middleware, you can use the following command: + +``` +rosrun assisted_navigation ir_drive_controller.py +``` +This will start the middleware module, allowing it to handle the communication and processing of data between different components of the system. + +With these commands, you should be able to run the sensor and middleware components of the system as needed. + +## Teleop_twist_keyboard +1. Make sure Teleop_twist_keyboard is installed + +2. make sure node Ir_drive_controller is running + + +3. run twist_to_pwm node using the following command +``` +rosrun assisted_navigation twist_to_pwm.py +``` + +4. In another terminal run the teleop node using the following command + +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py +``` + +Use the instruction in the terminal + +## Simulation +1. Make sure Gazebo is installed + +2. Make sure USE_SIMULATION is TRUE in the src/ir_drive_controller.py +*if its false then set to true then redo all the building steps + +``` +USE_SIMULATION = True +``` + +3. Run Gazebo using the following command +``` +gazebo +``` +4. Spawn the robot using the following command + +``` +roslaunch assisted_navigation spawn.launch +``` + +Use the teleop or joystick to control the robot + + + diff --git a/src/assisted_navigation/CMakeLists.txt b/src/assisted_navigation/CMakeLists.txt new file mode 100644 index 0000000..4ba15a4 --- /dev/null +++ b/src/assisted_navigation/CMakeLists.txt @@ -0,0 +1,217 @@ +cmake_minimum_required(VERSION 3.0.2) +project(assisted_navigation) + +## 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 COMPONENTS + gazebo_ros + geometry_msgs + roscpp + rospy + std_msgs + urdf +) + +## 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 +# geometry_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a 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 assisted_navigation +# CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp rospy std_msgs urdf +# 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}/assisted_navigation.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}_node src/assisted_navigation_node.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}) + +## 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 +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) +catkin_install_python(PROGRAMS + src/ir_drive_controller.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +catkin_install_python(PROGRAMS + src/twist_to_pwm.py + 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_assisted_navigation.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/assisted_navigation/launch/rviz.launch b/src/assisted_navigation/launch/rviz.launch new file mode 100644 index 0000000..2578d02 --- /dev/null +++ b/src/assisted_navigation/launch/rviz.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/assisted_navigation/launch/spawn.launch b/src/assisted_navigation/launch/spawn.launch new file mode 100644 index 0000000..99a8ba5 --- /dev/null +++ b/src/assisted_navigation/launch/spawn.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/src/assisted_navigation/launch/spawndefault.launch b/src/assisted_navigation/launch/spawndefault.launch new file mode 100644 index 0000000..41d63a3 --- /dev/null +++ b/src/assisted_navigation/launch/spawndefault.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/src/assisted_navigation/package.xml b/src/assisted_navigation/package.xml new file mode 100644 index 0000000..8e8375f --- /dev/null +++ b/src/assisted_navigation/package.xml @@ -0,0 +1,77 @@ + + + assisted_navigation + 0.0.0 + The assisted_navigation package + + + + + ronggurmwp + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + gazebo_ros + geometry_msgs + roscpp + rospy + std_msgs + urdf + gazebo_ros + geometry_msgs + roscpp + rospy + std_msgs + urdf + gazebo_ros + geometry_msgs + roscpp + rospy + std_msgs + urdf + + + + + + + + diff --git a/src/assisted_navigation/setup.py b/src/assisted_navigation/setup.py new file mode 100644 index 0000000..24cf322 --- /dev/null +++ b/src/assisted_navigation/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['ir_state', 'user_input_handler'], + package_dir={'': 'src'}, +) + +setup(**setup_args) \ No newline at end of file diff --git a/src/assisted_navigation/src/ir_drive_controller.py b/src/assisted_navigation/src/ir_drive_controller.py new file mode 100644 index 0000000..982e8d5 --- /dev/null +++ b/src/assisted_navigation/src/ir_drive_controller.py @@ -0,0 +1,120 @@ +""" +Usage: + +python ir_sensor_publisher.py +python ir_drive_controller.py + +and if you are using teleop keyboard use + +python twist_to_pwm.py +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +""" +import rospy +from std_msgs.msg import Int16, Int16MultiArray +from geometry_msgs.msg import Twist + +from user_input_handler import UserInputHandler +from ir_state import IRState + +# Parameters +rospy.set_param('USE_ASSISTED_DRIVE', True) # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_SIMULATION = False + +INPUT_PWM_MIN = 0 # Input PWM minimum value +INPUT_PWM_RANGE = 30 # Input PWM range value +OUTPUT_PWM_MIN = 0 # Output PWM minimum value +OUTPUT_PWM_RANGE = 20 # Output PWM range value + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" + +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" + +# Publish TWIST output mainly for simulation +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +# Set up publishers for each distance topic +IR_TOPIC = "/roboy/pinky/sensing/distance" +IR_FRONT_RIGHT_ID = 0 +IR_FRONT_LEFT_ID = 1 +IR_BACK_RIGHT_ID = 2 +IR_BACK_LEFT_ID = 3 + +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +irState = IRState() + +sign = lambda a: (a>0) - (a<0) + +def mapPwm(x, out_min, out_max): + """Map the x value 0.0 - 1.0 to out_min to out_max""" + return x * (out_max - out_min) + out_min + +def userInputCallback(msg, right): + """ + Callback funtion for user input. Takes the user input be it Twist_Teleop_Keyboard or joystick and based of variable Mode add moddification to speed + arg right is true if the callback is for the right motor and false if the callback is for the left motor + """ + # store user input + userInputHandler.setUserInput(msg, right) + + # call the current Mode control function to get the adjusted output + outputLinear, outputAngular = userInputHandler.getUserInput() + + AD = rospy.get_param('USE_ASSISTED_DRIVE') + # if the minimum distance is within a certaun threshold then brake + if((irState.get(IRState._FRONT_RIGHT_ID) or irState.get(IRState._FRONT_LEFT_ID)) and outputLinear > 0 and AD): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif ((irState.get(IRState._BACK_RIGHT_ID) or irState.get(IRState._BACK_RIGHT_ID)) and outputLinear < 0 and AD): # check if it about to collide in the back + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + outputLinear = 0 + + # publish the output to wheels + rospy.loginfo_throttle(5, "Publishing pwm..") + x = max(min(outputLinear, 1.0), -1.0) + z = max(min(outputAngular, 1.0), -1.0) + print("linear : ", x, ", angular : ",z) + + # publish TWIST output to simulaiton if USE_SIMULATION is true + if (USE_SIMULATION): + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output to the motor + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) + sign(x + z) * userInputHandler.PWM_MIN) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) + sign(x - z) * userInputHandler.PWM_MIN) + print("left : ", l, ", right : ",r) + + pub_motor_l.publish(l) + pub_motor_r.publish(r) + +def irSensorCallback(msg): + irState.set(msg.data) + return + +if __name__ == "__main__": + # init main loop + rospy.init_node('Ir_drive_controller') + + # initialize wheels publisher + pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + + if(USE_SIMULATION): + # initialize TWIST publisher for simulation + assisted_navigation_pub = rospy.Publisher(ASSISTED_NAVIGATION_TOPIC_OUTPUT, Twist, queue_size=3) + + # initialize subscriber for user input + user_input_sub_r = rospy.Subscriber(RIGHT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, True) + user_input_sub_l = rospy.Subscriber(LEFT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, False) + + #Initialize subscriber for Ir sensor + ir_sub = rospy.Subscriber(IR_TOPIC, Int16MultiArray, irSensorCallback) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() + \ No newline at end of file diff --git a/src/assisted_navigation/src/ir_drive_controller_simplified.py b/src/assisted_navigation/src/ir_drive_controller_simplified.py new file mode 100644 index 0000000..eaa3719 --- /dev/null +++ b/src/assisted_navigation/src/ir_drive_controller_simplified.py @@ -0,0 +1,100 @@ + + +# this is a simplified version of ir_drive_controller it didnt change the PWM input into a linear and angular insted just forward it after checking if its going to collide +""" +Usage: + +python ir_sensor_publisher.py +python ir_drive_controller.py + +and if you are using teleop keyboard use + +python twist_to_pwm.py +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +""" +import rospy +from std_msgs.msg import Int16, Int16MultiArray +from geometry_msgs.msg import Twist + +# Parameters +rospy.set_param('USE_EMERGENCYSTOP', True) # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP + +INPUT_PWM_MIN = 0 # Input PWM minimum value +INPUT_PWM_RANGE = 30 # Input PWM range value +OUTPUT_PWM_MIN = 0 # Output PWM minimum value +OUTPUT_PWM_RANGE = 20 # Output PWM range value + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" + +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" + +# Publish TWIST output mainly for simulation +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +# Set up publishers for each distance topic +IR_TOPIC = "/roboy/pinky/sensing/distance" +# these are the index of the infrared sensor published at IR_TOPIC +IR_FRONT_RIGHT_ID = 0 +IR_FRONT_LEFT_ID = 1 +IR_BACK_RIGHT_ID = 2 +IR_BACK_LEFT_ID = 3 + +# place holder to store ir sensor data +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT_ID = 0 + _FRONT_LEFT_ID = 1 + _BACK_RIGHT_ID = 2 + _BACK_LEFT_ID = 3 + def __init__(self): + self.ir_sensor = [0,0,0,0] + return + + def set(self, input): + self.ir_sensor = input + return + + def get(self,pos): + return self.ir_sensor[pos] + +irState = IRState() + +# IR sensor topic callback function +def irSensorCallback(msg): + irState.set(msg.data) + return + +def userInputCallback(msg, right): + if((not irState.get(irState._FRONT_RIGHT_ID) or not irState.get(irState._FRONT_LEFT_ID)) and msg.data > 0 and rospy.get_param('USE_EMERGENCYSTOP')): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + msg.data = 0 + elif ((not irState.get(irState._BACK_RIGHT_ID) or not irState.get(irState._BACK_LEFT_ID)) and msg.data < 0 and rospy.get_param('USE_EMERGENCYSTOP')): # check if it about to collide in the back + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + msg.data = 0 + + if(right): + pub_motor_r.publish(msg.data) + else: + pub_motor_l.publish(msg.data) + return + +if __name__ == "__main__": + # init main loop + rospy.init_node('Ir_drive_controller') + + # initialize wheels publisher + pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + + # initialize subscriber for user input + user_input_sub_r = rospy.Subscriber(RIGHT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, True) + user_input_sub_l = rospy.Subscriber(LEFT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, False) + + #Initialize subscriber for Ir sensor + ir_sub = rospy.Subscriber(IR_TOPIC, Int16MultiArray, irSensorCallback) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() \ No newline at end of file diff --git a/src/assisted_navigation/src/ir_sensor_publisher.py b/src/assisted_navigation/src/ir_sensor_publisher.py new file mode 100644 index 0000000..412fb86 --- /dev/null +++ b/src/assisted_navigation/src/ir_sensor_publisher.py @@ -0,0 +1,35 @@ +#!/usr/bin/python3 +import Jetson.GPIO as GPIO +import time + +import rospy +from std_msgs.msg import Int16MultiArray + + +SENSOR_PINS = [15,16,18,19,21,22,23,24] +TOPIC_NAME = "/roboy/pinky/sensing/distance" + +def setup_gpios(): + GPIO.setmode(GPIO.BOARD) + for pin in SENSOR_PINS: + GPIO.setup(pin, GPIO.IN) + +def publish_data(pub): + msg = Int16MultiArray() + for pin in SENSOR_PINS: + msg.data.append(GPIO.input(pin)) + pub.publish(msg) + +def main(): + setup_gpios() + + rospy.init_node("ir_sensor_publisher") + pub = rospy.Publisher(TOPIC_NAME, Int16MultiArray, queue_size=1) + rate = rospy.Rate(50) + rospy.loginfo("Infrared distance node is setup. Publishing data on " + TOPIC_NAME) + while not rospy.is_shutdown(): + publish_data(pub) + rate.sleep() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/assisted_navigation/src/ir_state.py b/src/assisted_navigation/src/ir_state.py new file mode 100644 index 0000000..d3c5d0c --- /dev/null +++ b/src/assisted_navigation/src/ir_state.py @@ -0,0 +1,16 @@ +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT_ID = 1 + _FRONT_LEFT_ID = 2 + _BACK_RIGHT_ID = 3 + _BACK_LEFT_ID = 4 + def __init__(self): + self.ir_sensor = [0,0,0,0] + return + + def set(self, input): + self.ir_sensor = input + return + + def get(self,pos): + return self.ir_sensor[pos] \ No newline at end of file diff --git a/src/assisted_navigation/src/twist_to_pwm.py b/src/assisted_navigation/src/twist_to_pwm.py new file mode 100644 index 0000000..6986e21 --- /dev/null +++ b/src/assisted_navigation/src/twist_to_pwm.py @@ -0,0 +1,57 @@ +# Usage: + + +# rosrun rosserial_python serial_node.py tcp +# plugin wheelchair power + +# cd esp-wheelchair +# python software/twist_to_pwm.py + +# start ROS joystick node - atk3 is for logitech joystick +# roslaunch teleop_twist_joy teleop.launch joy_config:=atk3 +# hold 1 and left joystick to drive (hold 2 to drive faster) + +# OR + +# alternatively run keyboard control +# rosrun teleop_twist_keyboard teleop_twist_keyboard.py +# read the commands for the keyboard! + + + +import rospy +from geometry_msgs.msg import Twist +from std_msgs.msg import Int16 + +PWM_MIN = 0 +PWMRANGE = 30 + +rospy.init_node("wheelchair_twist_converter") + +pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/input", Int16, queue_size=1) +pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/input", Int16, queue_size=1) + +sign = lambda a: (a>0) - (a<0) + +def mapPwm(x, out_min, out_max): + return x * (out_max - out_min) + out_min; + + +def cb(msg): + rospy.loginfo_throttle(5, "Publishing pwm..") + x = max(min(msg.linear.x, 1.0), -1.0) + z = max(min(msg.angular.z, 1.0), -1.0) + + l = (msg.linear.x - msg.angular.z) / 2.0 + r = (msg.linear.x + msg.angular.z) / 2.0 + + lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) + rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) + + pub_l.publish(int(sign(l)*lPwm)) + pub_r.publish(int(sign(r)*rPwm)) + +sub = rospy.Subscriber("/cmd_vel", Twist, cb) + +rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.spin() diff --git a/src/assisted_navigation/src/user_input_handler.py b/src/assisted_navigation/src/user_input_handler.py new file mode 100644 index 0000000..7bc2108 --- /dev/null +++ b/src/assisted_navigation/src/user_input_handler.py @@ -0,0 +1,48 @@ +from std_msgs.msg import Float64, Int16 + +class UserInputHandler: + """ Handles PWM input to linear and angular speed """ + PWM_MIN = 0 # PWM minimum value + PWMRANGE = 0 # PWM range value + + leftInput = 0 + leftIsDefined = False + rightInput = 0 + rightIsDefined = False + def __init__(self, PWM_MIN, PWMRANGE): + self.PWM_MIN = PWM_MIN + self.PWMRANGE = PWMRANGE + return + + def setUserInput(self, value, right): + """ Set the user input, store them at class member and define IsDefine as true """ + if (right): + self.rightInput = value + self.rightIsDefined = True + else : + self.leftInput = value + self.leftIsDefined = True + return + + def getUserInput(self): + """ Make sure User input is defined and return user input as linear and angular speed returned the user input in the range of -1 to 1""" + if(self.leftIsDefined and self.rightIsDefined): + inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + return inputLinear, inputAngular + + else: + inputLinear = 0 + inputAngular = 0 + return inputLinear, inputAngular + + def translate(self, value, leftMin, leftMax, rightMin, rightMax): + # calculate value range + leftRange = leftMax - leftMin + rightRange = rightMax - rightMin + + # Convert the left range into a 0-1 range + valueScaled = float(value - leftMin) / float(leftRange) + + # Convert the 0-1 range into a value in the right range. + return rightMin + (valueScaled * rightRange) diff --git a/src/assisted_navigation/urdf/m2wr.xacro b/src/assisted_navigation/urdf/m2wr.xacro new file mode 100644 index 0000000..de37222 --- /dev/null +++ b/src/assisted_navigation/urdf/m2wr.xacro @@ -0,0 +1,158 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Blue + + + Gazebo/Blue + + + + + false + true + 20 + joint_left_wheel + joint_right_wheel + 0.2 + 0.2 + 0.1 + /roboy/pinky/middleware/espchair/wheels/assisted_navigation + odom + odom + link_chassis + + + + + + 0 0 0.1 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/assisted_navigation/urdf/robody.xacro b/src/assisted_navigation/urdf/robody.xacro new file mode 100644 index 0000000..82c9d79 --- /dev/null +++ b/src/assisted_navigation/urdf/robody.xacro @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Blue + + + Gazebo/Blue + + + + + false + true + 20 + joint_left_wheel + joint_right_wheel + 0.2 + 0.2 + 0.1 + /roboy/pinky/middleware/espchair/wheels/assisted_navigation + odom + odom + link_chassis + + + + + + + 0 0 0.1 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/CMakeLists.txt b/src/ir_drive/CMakeLists.txt new file mode 100644 index 0000000..fd04d55 --- /dev/null +++ b/src/ir_drive/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ir_drive) + +## 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 COMPONENTS + geometry_msgs + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a 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 +# geometry_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a 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 ir_drive +# CATKIN_DEPENDS geometry_msgs rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ir_drive.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}_node src/ir_drive_node.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}) + +## 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 +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +catkin_install_python(PROGRAMS + src/ir_drive_controller.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +catkin_install_python(PROGRAMS + src/twist_to_pwm.py + 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_ir_drive.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/ir_drive/launch/rviz.launch b/src/ir_drive/launch/rviz.launch new file mode 100644 index 0000000..2578d02 --- /dev/null +++ b/src/ir_drive/launch/rviz.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/launch/spawn.launch b/src/ir_drive/launch/spawn.launch new file mode 100644 index 0000000..089680e --- /dev/null +++ b/src/ir_drive/launch/spawn.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/launch/spawndefault.launch b/src/ir_drive/launch/spawndefault.launch new file mode 100644 index 0000000..41d63a3 --- /dev/null +++ b/src/ir_drive/launch/spawndefault.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/launch/spawnhuman.launch b/src/ir_drive/launch/spawnhuman.launch new file mode 100644 index 0000000..4d07698 --- /dev/null +++ b/src/ir_drive/launch/spawnhuman.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/package.xml b/src/ir_drive/package.xml new file mode 100644 index 0000000..2e7a3f9 --- /dev/null +++ b/src/ir_drive/package.xml @@ -0,0 +1,68 @@ + + + ir_drive + 0.0.0 + The ir_drive package + + + + + ronggurmahedra + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + rospy + std_msgs + geometry_msgs + rospy + std_msgs + geometry_msgs + rospy + std_msgs + + + + + + + + diff --git a/src/ir_drive/setup.py b/src/ir_drive/setup.py new file mode 100644 index 0000000..24cf322 --- /dev/null +++ b/src/ir_drive/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['ir_state', 'user_input_handler'], + package_dir={'': 'src'}, +) + +setup(**setup_args) \ No newline at end of file diff --git a/src/ir_drive/src/ir_drive_controller.py b/src/ir_drive/src/ir_drive_controller.py new file mode 100644 index 0000000..b9664dc --- /dev/null +++ b/src/ir_drive/src/ir_drive_controller.py @@ -0,0 +1,120 @@ +""" +Usage: + +python ir_sensor_publisher.py +python ir_drive_controller.py + +and if you are using teleop keyboard use + +python twist_to_pwm.py +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +""" +import rospy +from std_msgs.msg import Int16, Int16MultiArray +from geometry_msgs.msg import Twist + +from user_input_handler import UserInputHandler +from ir_state import IRState + +# Parameters +rospy.set_param('USE_ASSISTED_DRIVE', True) # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_SIMULATION = False + +INPUT_PWM_MIN = 0 # Input PWM minimum value +INPUT_PWM_RANGE = 30 # Input PWM range value +OUTPUT_PWM_MIN = 0 # Output PWM minimum value +OUTPUT_PWM_RANGE = 20 # Output PWM range value + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" + +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" + +# Publish TWIST output mainly for simulation +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +# Set up publishers for each distance topic +IR_TOPIC = "/roboy/pinky/sensing/distance" +IR_FRONT_RIGHT_ID = 0 +IR_FRONT_LEFT_ID = 1 +IR_BACK_RIGHT_ID = 2 +IR_BACK_LEFT_ID = 3 + +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +irState = IRState() + +sign = lambda a: (a>0) - (a<0) + +def mapPwm(x, out_min, out_max): + """Map the x value 0.0 - 1.0 to out_min to out_max""" + return x * (out_max - out_min) + out_min + +def userInputCallback(msg, right): + """ + Callback funtion for user input. Takes the user input be it Twist_Teleop_Keyboard or joystick and based of variable Mode add moddification to speed + arg right is true if the callback is for the right motor and false if the callback is for the left motor + """ + # store user input + userInputHandler.setUserInput(msg, right) + + # call the current Mode control function to get the adjusted output + outputLinear, outputAngular = userInputHandler.getUserInput() + + AD = rospy.get_param('USE_ASSISTED_DRIVE') + # if the minimum distance is within a certaun threshold then brake + if((irState.get(IRState._FRONT_RIGHT_ID) or irState.get(IRState._FRONT_LEFT_ID)) and outputLinear > 0 and AD): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif ((irState.get(IRState._BACK_RIGHT_ID) or irState.get(IRState._BACK_RIGHT_ID)) and outputLinear < 0 and AD): # check if it about to collide in the back + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + outputLinear = 0 + + # publish the output to wheels + rospy.loginfo_throttle(5, "Publishing pwm..") + x = max(min(outputLinear, 1.0), -1.0) + z = max(min(outputAngular, 1.0), -1.0) + print("linear : ", x, ", angular : ",z) + + # publish TWIST output to simulaiton if USE_SIMULATION is true + if (USE_SIMULATION): + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output to the motor + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) + sign(x + z) * userInputHandler.PWM_MIN) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) + sign(x - z) * userInputHandler.PWM_MIN) + print("left : ", l, ", right : ",r) + + pub_motor_l.publish(l) + pub_motor_r.publish(r) + +def irSensorCallback(msg): + irState.set(msg.data) + return + +if __name__ == "__main__": + # init main loop + rospy.init_node('Ir_drive_controller') + + # initialize wheels publisher + pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + + if(USE_SIMULATION): + # initialize TWIST publisher for simulation + assisted_navigation_pub = rospy.Publisher(ASSISTED_NAVIGATION_TOPIC_OUTPUT, Twist, queue_size=3) + + # initialize subscriber for user input + user_input_sub_r = rospy.Subscriber(RIGHT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, True) + user_input_sub_l = rospy.Subscriber(LEFT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, False) + + #Initialize subscriber for Ir sensor + ir_sub = rospy.Subscriber(IR_TOPIC, Int16MultiArray, irSensorCallback) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() + \ No newline at end of file diff --git a/src/ir_drive/src/ir_drive_controller_simplified.py b/src/ir_drive/src/ir_drive_controller_simplified.py new file mode 100644 index 0000000..eaa3719 --- /dev/null +++ b/src/ir_drive/src/ir_drive_controller_simplified.py @@ -0,0 +1,100 @@ + + +# this is a simplified version of ir_drive_controller it didnt change the PWM input into a linear and angular insted just forward it after checking if its going to collide +""" +Usage: + +python ir_sensor_publisher.py +python ir_drive_controller.py + +and if you are using teleop keyboard use + +python twist_to_pwm.py +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +""" +import rospy +from std_msgs.msg import Int16, Int16MultiArray +from geometry_msgs.msg import Twist + +# Parameters +rospy.set_param('USE_EMERGENCYSTOP', True) # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP + +INPUT_PWM_MIN = 0 # Input PWM minimum value +INPUT_PWM_RANGE = 30 # Input PWM range value +OUTPUT_PWM_MIN = 0 # Output PWM minimum value +OUTPUT_PWM_RANGE = 20 # Output PWM range value + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" + +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" + +# Publish TWIST output mainly for simulation +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +# Set up publishers for each distance topic +IR_TOPIC = "/roboy/pinky/sensing/distance" +# these are the index of the infrared sensor published at IR_TOPIC +IR_FRONT_RIGHT_ID = 0 +IR_FRONT_LEFT_ID = 1 +IR_BACK_RIGHT_ID = 2 +IR_BACK_LEFT_ID = 3 + +# place holder to store ir sensor data +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT_ID = 0 + _FRONT_LEFT_ID = 1 + _BACK_RIGHT_ID = 2 + _BACK_LEFT_ID = 3 + def __init__(self): + self.ir_sensor = [0,0,0,0] + return + + def set(self, input): + self.ir_sensor = input + return + + def get(self,pos): + return self.ir_sensor[pos] + +irState = IRState() + +# IR sensor topic callback function +def irSensorCallback(msg): + irState.set(msg.data) + return + +def userInputCallback(msg, right): + if((not irState.get(irState._FRONT_RIGHT_ID) or not irState.get(irState._FRONT_LEFT_ID)) and msg.data > 0 and rospy.get_param('USE_EMERGENCYSTOP')): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + msg.data = 0 + elif ((not irState.get(irState._BACK_RIGHT_ID) or not irState.get(irState._BACK_LEFT_ID)) and msg.data < 0 and rospy.get_param('USE_EMERGENCYSTOP')): # check if it about to collide in the back + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + msg.data = 0 + + if(right): + pub_motor_r.publish(msg.data) + else: + pub_motor_l.publish(msg.data) + return + +if __name__ == "__main__": + # init main loop + rospy.init_node('Ir_drive_controller') + + # initialize wheels publisher + pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + + # initialize subscriber for user input + user_input_sub_r = rospy.Subscriber(RIGHT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, True) + user_input_sub_l = rospy.Subscriber(LEFT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, False) + + #Initialize subscriber for Ir sensor + ir_sub = rospy.Subscriber(IR_TOPIC, Int16MultiArray, irSensorCallback) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() \ No newline at end of file diff --git a/src/ir_drive/src/ir_sensor_publisher.py b/src/ir_drive/src/ir_sensor_publisher.py new file mode 100644 index 0000000..412fb86 --- /dev/null +++ b/src/ir_drive/src/ir_sensor_publisher.py @@ -0,0 +1,35 @@ +#!/usr/bin/python3 +import Jetson.GPIO as GPIO +import time + +import rospy +from std_msgs.msg import Int16MultiArray + + +SENSOR_PINS = [15,16,18,19,21,22,23,24] +TOPIC_NAME = "/roboy/pinky/sensing/distance" + +def setup_gpios(): + GPIO.setmode(GPIO.BOARD) + for pin in SENSOR_PINS: + GPIO.setup(pin, GPIO.IN) + +def publish_data(pub): + msg = Int16MultiArray() + for pin in SENSOR_PINS: + msg.data.append(GPIO.input(pin)) + pub.publish(msg) + +def main(): + setup_gpios() + + rospy.init_node("ir_sensor_publisher") + pub = rospy.Publisher(TOPIC_NAME, Int16MultiArray, queue_size=1) + rate = rospy.Rate(50) + rospy.loginfo("Infrared distance node is setup. Publishing data on " + TOPIC_NAME) + while not rospy.is_shutdown(): + publish_data(pub) + rate.sleep() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/ir_drive/src/ir_state.py b/src/ir_drive/src/ir_state.py new file mode 100644 index 0000000..d3c5d0c --- /dev/null +++ b/src/ir_drive/src/ir_state.py @@ -0,0 +1,16 @@ +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT_ID = 1 + _FRONT_LEFT_ID = 2 + _BACK_RIGHT_ID = 3 + _BACK_LEFT_ID = 4 + def __init__(self): + self.ir_sensor = [0,0,0,0] + return + + def set(self, input): + self.ir_sensor = input + return + + def get(self,pos): + return self.ir_sensor[pos] \ No newline at end of file diff --git a/src/ir_drive/src/twist_to_pwm.py b/src/ir_drive/src/twist_to_pwm.py new file mode 100644 index 0000000..9f7bba6 --- /dev/null +++ b/src/ir_drive/src/twist_to_pwm.py @@ -0,0 +1,57 @@ +# Usage: + + +# rosrun rosserial_python serial_node.py tcp +# plugin wheelchair power + +# cd esp-wheelchair +# python software/twist_to_pwm.py + +# start ROS joystick node - atk3 is for logitech joystick +# roslaunch teleop_twist_joy teleop.launch joy_config:=atk3 +# hold 1 and left joystick to drive (hold 2 to drive faster) + +# OR + +# alternatively run keyboard control +# rosrun teleop_twist_keyboard teleop_twist_keyboard.py +# read the commands for the keyboard! + + + +import rospy +from geometry_msgs.msg import Twist +from std_msgs.msg import Int16 + +PWM_MIN = 0 +PWMRANGE = 30 + +rospy.init_node("wheelchair_twist_converter") + +pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/input", Int16, queue_size=1) +pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/input", Int16, queue_size=1) + +sign = lambda a: (a>0) - (a<0) + +def mapPwm(x, out_min, out_max): + return x * (out_max - out_min) + out_min; + + +def cb(msg): + rospy.loginfo_throttle(5, "Publishing pwm..") + x = max(min(msg.linear.x, 1.0), -1.0) + z = max(min(msg.angular.z, 1.0), -1.0) + + l = (msg.linear.x - msg.angular.z) / 2.0 + r = (msg.linear.x + msg.angular.z) / 2.0 + + lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) + rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) + + pub_l.publish(int(sign(l)*lPwm)) + pub_r.publish(int(sign(r)*rPwm)) + +sub = rospy.Subscriber("/cmd_vel", Twist, cb) + +rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.spin() diff --git a/src/ir_drive/src/user_input_handler.py b/src/ir_drive/src/user_input_handler.py new file mode 100644 index 0000000..7bc2108 --- /dev/null +++ b/src/ir_drive/src/user_input_handler.py @@ -0,0 +1,48 @@ +from std_msgs.msg import Float64, Int16 + +class UserInputHandler: + """ Handles PWM input to linear and angular speed """ + PWM_MIN = 0 # PWM minimum value + PWMRANGE = 0 # PWM range value + + leftInput = 0 + leftIsDefined = False + rightInput = 0 + rightIsDefined = False + def __init__(self, PWM_MIN, PWMRANGE): + self.PWM_MIN = PWM_MIN + self.PWMRANGE = PWMRANGE + return + + def setUserInput(self, value, right): + """ Set the user input, store them at class member and define IsDefine as true """ + if (right): + self.rightInput = value + self.rightIsDefined = True + else : + self.leftInput = value + self.leftIsDefined = True + return + + def getUserInput(self): + """ Make sure User input is defined and return user input as linear and angular speed returned the user input in the range of -1 to 1""" + if(self.leftIsDefined and self.rightIsDefined): + inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + return inputLinear, inputAngular + + else: + inputLinear = 0 + inputAngular = 0 + return inputLinear, inputAngular + + def translate(self, value, leftMin, leftMax, rightMin, rightMax): + # calculate value range + leftRange = leftMax - leftMin + rightRange = rightMax - rightMin + + # Convert the left range into a 0-1 range + valueScaled = float(value - leftMin) / float(leftRange) + + # Convert the 0-1 range into a value in the right range. + return rightMin + (valueScaled * rightRange) diff --git a/src/ir_drive/urdf/human.urdf b/src/ir_drive/urdf/human.urdf new file mode 100644 index 0000000..5c00c3f --- /dev/null +++ b/src/ir_drive/urdf/human.urdf @@ -0,0 +1,1761 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/urdf/m2wr.xacro b/src/ir_drive/urdf/m2wr.xacro new file mode 100644 index 0000000..de37222 --- /dev/null +++ b/src/ir_drive/urdf/m2wr.xacro @@ -0,0 +1,158 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Blue + + + Gazebo/Blue + + + + + false + true + 20 + joint_left_wheel + joint_right_wheel + 0.2 + 0.2 + 0.1 + /roboy/pinky/middleware/espchair/wheels/assisted_navigation + odom + odom + link_chassis + + + + + + 0 0 0.1 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ir_drive/urdf/robody.xacro b/src/ir_drive/urdf/robody.xacro new file mode 100644 index 0000000..82c9d79 --- /dev/null +++ b/src/ir_drive/urdf/robody.xacro @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Blue + + + Gazebo/Blue + + + + + false + true + 20 + joint_left_wheel + joint_right_wheel + 0.2 + 0.2 + 0.1 + /roboy/pinky/middleware/espchair/wheels/assisted_navigation + odom + odom + link_chassis + + + + + + + 0 0 0.1 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + 0 + 0 + 2.0 + 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/test/tof_test.py b/test/tof_test.py new file mode 100644 index 0000000..c96b975 --- /dev/null +++ b/test/tof_test.py @@ -0,0 +1,60 @@ +import unittest +import rospy + +from std_msgs.msg import Float64 +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import Twist +import pcl +import ros_numpy +import numpy as np +import pcl.pcl_visualization +import sys +sys.path.insert(1, '../esp-wheelchair/src') + +from manual_control import * +from repelent_field_control import * + +class Assisted_Navigation_Test(unittest.TestCase): + """ Class to test use cases of assisted navigation functions """ + + def test_Repelent_Field(self): + """ Test the Repelent Field class """ + repelentMode = RepelentMode() + inputLinear = 10 + inputAngular = 11 + minDistFront = 0.01 + minDistBack = 10 + repelentMode.setDistanceFront(minDistFront) + repelentMode.setDistanceBack(minDistBack) + outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) + # test speed when going forward + self.assertEqual((outputLiner, outputAngular), (0.01, inputAngular)) + repelentMode = RepelentMode() + inputLinear = -1 + inputAngular = 2 + minDistFront = 2 + minDistBack = 0.1 + repelentMode.setDistanceBack(minDistFront) + repelentMode.setDistanceBack(minDistBack) + outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) + # test speed when going backward + self.assertEqual((outputLiner, outputAngular), (-0.1, inputAngular)) + + + def test_Manual_Mode(self): + """ Test Manual Mode should just return the same value """ + manualMode = ManualMode() + inputLinear = 10 + inputAngular = 1 + self.assertEqual(manualMode.control(inputLinear,inputAngular), (inputLinear, inputAngular)) + + inputLinear = -10 + inputAngular = 1 + self.assertEqual(manualMode.control(inputLinear,inputAngular), (inputLinear, inputAngular)) + + inputLinear = 0 + inputAngular = -1 + self.assertEqual(manualMode.control(inputLinear,inputAngular), (inputLinear, inputAngular)) + +if __name__ == '__main__': + unittest.main()