Integrate ToF Sensor, Implement emergency Brake, and repellent field

.gitignore

@@ -1,6 +1,6 @@
 software/build
 
 /*cache*
-
+**/__pycache__
 **/*.swp
 **/*.swo

scripts/assistedNavigation.bash

@@ -0,0 +1,8 @@
+# this script run all the necessary setup to run the assisted navigation program using the teleop keyboard
+
+export ROS_HOSTNAME=192.168.0.124
+# 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.0.124:11311
+roscore &
+python3 ../src/tof.py & 
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py

src/README.md

@@ -0,0 +1,28 @@
+# Assisted Navigation
+
+## Prerequisites
+- install ROS1 Melodic  
+
+## How to Run 
+Setup ToF according to this documentation : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software
+
+Or 
+
+Use Bagfile provided in this [link](https://drive.google.com/drive/folders/15IJ5Bk0Abo6tRJbfVam8PI72EpSFbLXI?usp=sharing)
+
+to use the bagfile, run 
+```
+rosbag play -l ToFSensor_sample.bag /royale_camera_driver/pointcloud:=/tof1_driver/point_cloud &
+rosbag play -l ToFSensor_sample2.bag /royale_camera_driver/pointcloud:=/tof2_driver/point_cloud
+```
+
+And to run the assisted navigation node
+```
+# if you havent have any rosmaster running and all the necessary Networking setup if you are using the ToFsensor
+roscore &
+
+# initialize the assisted navigation node with teleop keyboard 
+python3 ../src/tof.py & 
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+
+```

src/manual_control.py

@@ -0,0 +1,7 @@
+class ManualMode:
+    """ Manual Mode no modification between user input and output """
+    def __init__(self):
+        return
+
+    def control(self, inputLinear, inputAngular):
+        return inputLinear,inputAngular

src/repelent_field_control.py

@@ -0,0 +1,28 @@
+class RepelentMode:
+    """ Repelent field mode get slower as the robot get near an obstacle """
+    DistFront = 1
+    DistBack = 1
+    def __init__(self):
+        return
+
+    def setDistanceFront(self, distance):
+        """ Set member variable DistFront """
+        self.DistFront = distance
+    def getDistanceFront(self):
+        """ Set member variable DistFront """
+        return self.DistFront 
+
+    def setDistanceBack(self, distance):
+        """ Set member variable DistBack """
+        self.DistBack = distance
+    def getDistanceBack(self):
+        """ Set member variable DistFront """
+        return self.DistBack 
+    def control(self, inputLinear, inputAngular):
+        """ give an output based of the distance to an obstacle """
+        if inputLinear >= 0:
+            outputLinear = self.DistFront
+        elif inputLinear < 0:
+            outputLinear = -1*self.DistBack
+
+        return outputLinear,inputAngular

src/tof.py

@@ -0,0 +1,149 @@
+"""
+Usage:
+
+cd esp-wheelchair
+python3 software/tof.py
+
+"""
+import rospy
+from std_msgs.msg import Float64, Int16
+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
+from manual_control import * 
+from repelent_field_control import *
+
+# Parameters
+USEVISUAL = True # USEVISUAL if true will open a window that show the ToF sensor output
+PWM_MIN = 5 # PWM minimum value
+PWMRANGE = 40 # PWM range value
+EMERGENCYSTOPTHRESHOLD = 0.1 # emergency stop threshold roboy will stop if it detect a point below the thereshold
+
+
+# variable initialization
+if(USEVISUAL):
+    viewer_front = pcl.pcl_visualization.PCLVisualizering()
+    viewer_back = pcl.pcl_visualization.PCLVisualizering()
+inputLinear = None
+inputAngular = None
+manualMode = ManualMode()
+repelentMode = RepelentMode()
+Mode = manualMode
+
+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 pointCloudToNumpyArray(point_Cloud):
+    """ convert a ROS's pointcloud data structure to a numpy array"""
+    height = point_Cloud.shape[0]
+    width = point_Cloud.shape[1]
+    np_points = np.zeros((height * width, 3), dtype=np.float32)
+    np_points[:, 0] = np.resize(point_Cloud['x'], height * width)
+    np_points[:, 1] = np.resize(point_Cloud['y'], height * width)
+    np_points[:, 2] = np.resize(point_Cloud['z'], height * width)
+    return np_points
+
+def visualizePointCloud(viewer ,point_Cloud):
+    """ visualize a numpy point cloud to a viewer """
+    p = pcl.PointCloud(np.array(point_Cloud, dtype=np.float32))
+    viewer.AddPointCloud(p, b'scene_cloud_front', 0)
+    viewer.SpinOnce()
+    viewer.RemovePointCloud( b'scene_cloud_front', 0)
+
+
+def pointCloudFrontCallback(msg):
+    """ Callback function for front ToF sensor """
+    # change from pointcloud2 to numpy
+    pc = ros_numpy.numpify(msg)
+    front_Pointcloud_array = pointCloudToNumpyArray(pc)
+    # visualize if USEVISUAL is TRUE 
+    if(USEVISUAL):
+        visualizePointCloud(viewer_front, front_Pointcloud_array)
+    # find the nearest point and store it at repelent Class
+    minDist_front =  np.nanmin(front_Pointcloud_array[:,2])
+    repelentMode.setDistanceFront(minDist_front)
+
+def pointCloudBackCallback(msg):
+    """ Callback function for back ToF sensor """
+    # change from pointcloud2 to numpy
+    pc = ros_numpy.numpify(msg)
+    back_Pointcloud_array = pointCloudToNumpyArray(pc)
+    # visualize if USEVISUAL is TRUE 
+    if(USEVISUAL): 
+        visualizePointCloud(viewer_back, back_Pointcloud_array)
+    # find the nearest point and store it at repelent Class
+    minDist_back = np.nanmin(back_Pointcloud_array[:, 2])
+    repelentMode.setDistanceBack(minDist_back)
+
+def userInputCallback(msg):   
+    """ 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 """
+    # store user input
+    inputLinear = msg.linear.x
+    inputAngular = msg.angular.z
+
+    # call the current Mode control function
+    outputLinear,outputAngular = Mode.control(inputLinear,inputAngular)
+
+    # if the minimum distance is within a certaun threshold then brake
+    if(repelentMode.getDistanceFront() < EMERGENCYSTOPTHRESHOLD and inputLinear > 0): # this is the front ToF
+        print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE")
+        outputLinear = 0
+    elif (repelentMode.getDistanceBack() < EMERGENCYSTOPTHRESHOLD and inputLinear < 0): # this is the back ToF
+        print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE")
+        outputLinear = 0
+    # Check if wheelchair_emergency_stopped is defined 
+    if rospy.has_param('wheelchair_emergency_stopped'):
+        # check if wheelchair_emergency_stopped is TRUE
+        if rospy.get_param('wheelchair_emergency_stopped'):
+            return
+
+    # publish the TWIST output to simulation
+    twist = Twist()
+    twist.linear.x = outputLinear
+    twist.angular.z = outputAngular
+    assisted_navigation_pub.publish(twist)
+
+    # 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)
+
+    l = (outputLinear - outputAngular) / 2.0
+    r = (outputLinear + outputAngular) / 2.0
+
+    lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE)
+    rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE)
+    print(" left : ", sign(l)*lPwm, ", right : ",sign(r)*rPwm)
+    pub_l.publish(sign(l)*lPwm)
+    pub_r.publish(sign(r)*rPwm)
+
+if __name__ == "__main__":
+    # init main loop
+    rospy.init_node('assisted_Navigation')
+
+    # initialize mode with manual mode
+    Mode = manualMode
+
+    # initialize wheels publisher
+    pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left", Int16, queue_size=1)
+    pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right", Int16, queue_size=1)
+
+    # initialize TWIST publisher for simulation
+    assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10)
+
+    # initialize TWIST subscriber for user input 
+    user_input_sub = rospy.Subscriber('/cmd_vel', Twist, userInputCallback)
+
+    # initialize pointlcloud subscriber for ToF sensor
+    point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudFrontCallback)
+    point_cloud__back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudBackCallback)
+
+    print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...")
+    rospy.spin()
+

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