Integrate ToF Sensor, Implement emergency Brake, and repellent field

software/README.md

@@ -1 +1,115 @@
+# ToF Setup
 
+documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software
+## Variables : 
+
+MAC ADDRESS ToF 1 : 3C:FB:96:DC:59:7E
+MAC ADDRESS ToF 2 : 3C:FB:96:DC:59:7F
+
+ROS_MASTER_URI=http://192.168.0.104:11311
+
+For this documentation we use this Router Configuration : 
+
+
+TOF1
+MAC ADDRESS: 3C:FB:96:DC:59:7E
+IP: 192.168.0.122
+
+TOF2
+MAC ADDRESS: 3C:FB:96:DC:59:7F
+IP: 192.168.0.123
+
+MASTER 
+IP : 192.168.0.124
+
+## Setup TOF 1
+
+to setup tof1 do these command :
+
+cd etc
+nano rc.local
+
+and then add rc.local with these commands: 
+
+#stops royale viewer entity
+systemctl stop royaleviewer
+source /opt/ros/melodic/setup.sh
+
+# 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
+
+# if you use other ip for ToF2 then do  “export ROS_HOSTNAME={TOF2_IP}”
+export ROS_HOSTNAME=192.168.0.122
+roslaunch royale_ros_sample camera_driver.launch node_name:=”tof1”
+
+exit 0
+
+Reset the ToF sensor then ToF should publish data to /tof1/...
+
+## Setup TOF 2
+
+to setup tof2 do these command :
+
+cd etc
+nano rc.local
+
+and then add rc.local with these commands: 
+
+#stops royale viewer entity
+systemctl stop royaleviewer
+source /opt/ros/melodic/setup.sh
+
+#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
+
+#if you use other ip for ToF2 then do  “export ROS_HOSTNAME={TOF2_IP}”
+export ROS_HOSTNAME=192.168.0.123
+roslaunch royale_ros_sample camera_driver.launch node_name:=”tof2”
+
+exit 0
+
+Reset the ToF sensor then ToF should publish data to /tof2/...
+
+## MASTER
+
+Do these command on the ROS master device:
+
+#if you use other ip for ros master then do  “export ROS_HOSTNAME={ROS_MASTER_IP}”
+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
+
+these code start the ROS MASTER 
+
+
+
+on a new terminal : 
+
+git clone -b an_ToF_emergencyBrake https://github.com/ronggurmahendra/esp-wheelchair.git
+cd esp-wheelchair
+python3 software/tof_potential_field.py
+
+on a new terminal
+
+cd esp-wheelchair
+python3 software/twist_to_pwm.py
+
+on a new terminal 
+
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+
+SIMULATION
+
+to run the simulation 
+
+#asuming you already done the http://wiki.ros.org/catkin/Tutorials/create_a_workspace  if not then do so 
+cd catkin_ws/src
+git clone https://github.com/ronggurmahendra/Robody_Sim 
+cd ..
+catkin_make 
+source devel/setup.sh
+
+#run gazebo world of your choice
+roslaunch gazebo_ros shapes_world.launch

software/assistedNavigation.bash

@@ -0,0 +1,7 @@
+#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 software/twist_to_pwm.py &
+python3 software/tof.py & 
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py

software/manual.py

@@ -0,0 +1,6 @@
+class ManualMode:
+    def __init__(self):
+        return
+
+    def control(self, inputLinear, inputAngular):
+        return inputLinear,inputAngular

software/repelent_field.py

@@ -0,0 +1,11 @@
+class RepelentMode:
+    def __init__(self):
+        return
+
+    def control(self, inputLinear, inputAngular, minDistFront, minDistBack):
+        if inputLinear >= 0:
+            outputLinear = minDistFront
+        elif inputLinear < 0:
+            outputLinear = -1*minDistBack
+
+        return outputLinear,inputAngular

software/test/tof_test.py

@@ -0,0 +1,55 @@
+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/software')
+
+from manual import * 
+from repelent_field import * 
+
+class Assisted_Navigation_Test(unittest.TestCase):
+    def test_Repelent_Field(self):
+        repelentMode = RepelentMode()
+        inputLinear = 10
+        inputAngular = 11
+        minDistFront = 0.01
+        minDistBack = 10
+
+        outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack)
+        # test
+        # assert(outputLiner == 0 and outputAngular == outputAngular )
+        self.assertEqual((outputLiner, outputAngular), (0.01, inputAngular))
+        repelentMode = RepelentMode()
+        inputLinear = -1
+        inputAngular = 2
+        minDistFront = 2
+        minDistBack = 0.1
+
+        outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack)
+        # test
+        self.assertEqual((outputLiner, outputAngular), (-0.1, inputAngular))
+
+
+    def test_Manual_Mode(self):
+        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()

software/tof.py

@@ -0,0 +1,104 @@
+# Usage:
+
+# cd esp-wheelchair
+# python3 software/tof.py
+
+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
+from manual import * 
+from repelent_field import *
+useVisual = False
+
+emergencyStopThreshold = 0.1
+if(useVisual):
+    viewer_front = pcl.pcl_visualization.PCLVisualizering()
+    viewer_back = pcl.pcl_visualization.PCLVisualizering()
+minDist_front = 9999
+minDist_back = 9999
+inputLinear = None
+inputAngular = None
+manualMode = ManualMode()
+repelentMode = RepelentMode()
+
+def getNearestDistance(points):
+    min = 9999
+    for i in range(len(points)):
+        if(min > points[i][2]):
+            min = points[i][2]
+    return min
+
+def point_cloud_front_callback(msg):
+    # change from pointcloud2 to numpy
+    pc = ros_numpy.numpify(msg)
+    height = pc.shape[0]
+    width = pc.shape[1]
+    np_points = np.zeros((height * width, 3), dtype=np.float32)
+    np_points[:, 0] = np.resize(pc['x'], height * width)
+    np_points[:, 1] = np.resize(pc['y'], height * width)
+    np_points[:, 2] = np.resize(pc['z'], height * width)
+    if(useVisual):
+        p = pcl.PointCloud(np.array(np_points, dtype=np.float32))
+        viewer_front.AddPointCloud(p, b'scene_cloud_front', 0)
+        viewer_front.SpinOnce()
+        viewer_front.RemovePointCloud( b'scene_cloud_front', 0)
+    # find the nearest point
+    global minDist_front
+    minDist_front = getNearestDistance(np_points)
+
+def point_cloud_back_callback(msg):
+    # change from pointcloud2 to numpy
+    pc = ros_numpy.numpify(msg)
+    height = pc.shape[0]
+    width = pc.shape[1]
+    np_points = np.zeros((height * width, 3), dtype=np.float32)
+    np_points[:, 0] = np.resize(pc['x'], height * width)
+    np_points[:, 1] = np.resize(pc['y'], height * width)
+    np_points[:, 2] = np.resize(pc['z'], height * width)
+    if(useVisual):
+        p = pcl.PointCloud(np.array(np_points, dtype=np.float32))
+        viewer_back.AddPointCloud(p, b'scene_cloud_back', 0)
+        viewer_back.SpinOnce()
+        viewer_back.RemovePointCloud( b'scene_cloud_back', 0)
+    # find the nearest point
+    global minDist_back
+    minDist_back = getNearestDistance(np_points)
+
+def user_input_callback(msg):   
+    inputLinear = msg.linear.x
+    inputAngular = msg.angular.z
+
+# manual mode
+    outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular)
+
+    if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF
+        print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE")
+        outputLinear = 0
+    elif (minDist_back < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF
+        print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE")
+        outputLinear = 0
+
+# reppelent field 
+    # outputLinear,outputAngular = repelentMode.control(inputLinear,inputAngular,minDist_front,minDist_back)
+
+    twist = Twist()
+    twist.linear.x = outputLinear
+    twist.angular.z = outputAngular
+    assisted_navigation_pub.publish(twist)
+
+rospy.init_node('assisted_Navigation')
+
+assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10)
+user_input_sub = rospy.Subscriber('/cmd_vel', Twist, user_input_callback)
+# point_cloud_sub = rospy.Subscriber('/royale_camera_driver/point_cloud', PointCloud2, point_cloud_callback)
+
+point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, point_cloud_front_callback)
+point_cloud__back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, point_cloud_back_callback)
+
+print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...")
+rospy.spin()

software/twist_to_pwm.py

@@ -1,11 +1,10 @@
 # Usage:
 
-
 # rosrun rosserial_python serial_node.py tcp
 # plugin wheelchair power
 
 # cd esp-wheelchair
-# python software/twist_to_pwm.py
+# python3 software/twist_to_pwm.py
 
 # start ROS joystick node - atk3 is for logitech joystick
 # roslaunch teleop_twist_joy teleop.launch joy_config:=atk3
@@ -38,25 +37,22 @@ def mapPwm(x, out_min, out_max):
 
 
 def cb(msg):
-	if not rospy.get_param('wheelchair_emergency_stopped'):
-		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)
+	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)
 
-		pub_l.publish(sign(l)*lPwm)
-		pub_r.publish(sign(r)*rPwm)
-	else:
-		rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel")
+	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)
+	print(" left : ", sign(l)*lPwm, ", right : ",sign(r)*rPwm)
+	pub_l.publish(sign(l)*lPwm)
+	pub_r.publish(sign(r)*rPwm)
 
 
-sub = rospy.Subscriber("/cmd_vel", Twist, cb)
+InputTopic = "/roboy/pinky/middleware/espchair/wheels/assisted_navigation"
+sub = rospy.Subscriber(InputTopic, Twist, cb)
 
-rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...")
-rospy.spin()
+rospy.loginfo("Subscribed to " + InputTopic + ", will publish wheelchair PWM. Spinning...")
+rospy.spin()