Integrate ToF Sensor, Implement emergency Brake, and repellent field #4

ronggurmahendra · 2023-02-08T00:08:03Z

This pull Implement python file that initialize a ROS Node called assisted_Navigation. The objective is to integrate 2 time-of-flight camera into the mobile base of Roboy and prevent the driver from driving into obstacles with the whole body
feature of the assisted_Navigation node includes :

emergency brake after a certain threshold of distance of ToF sensor reached
manual mode(the same system as before) -> map the user input "cmd_vel_" to the left and right motor
potential field mode -> slows down as it get near an obstacle

…ntial field node

missxa

missing architecture diagram and explanation of logic + see comments in the code

missxa · 2023-02-14T10:44:53Z

software/README.md


+documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software
+## Variables : 


everything related to ToF sensor setup belongs to your team's documentation page and not this repository

missxa · 2023-02-14T10:46:12Z

software/README.md


+documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software
+## Variables : 
+


markdown is used incorrectly, code is not highlighted and headlines are not used correctly
refer to https://github.com/Roboy/esp-wheelchair/blob/5c101553f75b88ecdbaae51f26349fd8e440acda/README.md for an example

missxa · 2023-02-14T10:47:34Z

software/assistedNavigation.bash

@@ -0,0 +1,7 @@
+#export ROS_HOSTNAME=192.168.0.124


this script belongs to scripts folder and must have a description on top about what exactly it does

missxa · 2023-02-14T10:49:59Z

software/manual.py

@@ -0,0 +1,6 @@
+class ManualMode:


software folder is dedicated to the code that is running on the microcontroller of the mobile platform. for you python scripts, please create src folder in the root of this repository and move them there. manual.py is not a self-descriptive name. manual_control would be more appropriate. dito for repellent field (also please check your english for typos, there are quite a few of them).

missxa · 2023-02-14T10:50:16Z

software/manual.py

@@ -0,0 +1,6 @@
+class ManualMode:


docstrings for classes are missing, please add

missxa · 2023-02-14T10:55:23Z

software/tof.py

+    for i in range(len(points)):
+        if(min > points[i][2]):
+            min = points[i][2]
+    return min


use numpy methods to find min

missxa · 2023-02-14T10:55:46Z

software/tof.py

+        viewer_front.SpinOnce()
+        viewer_front.RemovePointCloud( b'scene_cloud_front', 0)
+    # find the nearest point
+    global minDist_front


not global variables are allowed

missxa · 2023-02-14T10:57:00Z

software/tof.py

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


copy paste antipattern, identical to point_cloud_front_callback, rewrite to 1 function that can be used for front and back

missxa · 2023-02-14T10:58:02Z

software/tof.py

+# 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)


comments missing

missxa · 2023-02-14T10:59:48Z

software/tof.py

+
+rospy.init_node('assisted_Navigation')
+
+assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10)


wrong topic name and message. /roboy/pinky/middleware/espchair/wheels/left and /roboy/pinky/middleware/espchair/wheels/right are used to control the wheelchair

…ive controller, fix bug with topic /roboy/pinky/middleware/espchair/wheels/dist/front&back

…to using an array

…lified version of the middleware which basically just forward the message

… an_ToF

ronggurmahendra added 18 commits January 31, 2023 22:48

adding potential field node and modifying twist_to_pwm to sub to pote…

a507572

…ntial field node

adding potential field as consideration

358cc26

disable the dynamic speed and implement only the emergency brake

4bade8e

cahnge the emergencyStopThreshold

a61cc99

refactor and remove unnecesarry code

fe9a3e8

update README.md

d2a9d44

update README.md

1d44b5a

changes so that it consider current speed

d8f340b

deleting a unused file

5c86e50

implement manual mode

3a75ac3

fix bug viewer

a182f4e

fix bug at viewer

5bf5994

refactor

adbe730

adding repelent field

942cc71

implement repelent field

e5efc7a

editting tof_tes

e876461

adding unit test

9bdb4ad

adding test file

197b61d

ronggurmahendra requested a review from missxa February 8, 2023 00:08

ronggurmahendra self-assigned this Feb 8, 2023

ronggurmahendra changed the title ~~Integrate ToF Sensor, IMplement emergency Brake, repellent field~~ Integrate ToF Sensor, Implement emergency Brake, and repellent field Feb 8, 2023

add test file cases

5c10155

ronggurmahendra marked this pull request as ready for review February 8, 2023 16:06

missxa requested changes Feb 14, 2023

View reviewed changes

ronggurmahendra added 6 commits February 16, 2023 02:47

refactor : move file to different directories

748cf03

refactor : update readme.md, delete cache

ada812e

refactor : add comment to bash file

31155ec

refactor : change file name

7e9d54b

dev : change to numpy min

73ff751

refactor : use function at point cloud callback

9558472

ronggurmahendra added 30 commits March 19, 2023 17:09

resolve conflict

0fad87f

split assisted navigation to 2 node

eabad4e

adding drive mode as a class, refactor assisted navigation iao and dr…

8ada88f

…ive controller, fix bug with topic /roboy/pinky/middleware/espchair/wheels/dist/front&back

change parameter to use somulation and add simulation to bash file

638f887

change namefile of ToF implementation and add a IR sensor version

1b0ef7d

fix a mistake with output linear

398ef70

ir publisher overhaul

a5ee64f

refactor IRversion of assisted navigation

1f7ddbf

change file name ro be more consistent

d9af7dd

delete previous impelmentation using ToF and change the IR publisher …

7e23a16

…to using an array

add a stand alone python script all in one for drive controller

6f17f5a

remove unnesesarry code and modify according to IRsensor publisher

904dfd2

fix minor bug

b6aa824

resolving conflict

6876bcd

change the index

4d6cadd

add PMWmin to the final published output of the motor and make a simp…

2efcda0

…lified version of the middleware which basically just forward the message

Merge branch 'an_ToF' of https://github.com/Roboy/esp-wheelchair into…

d0f5537

… an_ToF

adding USE_EMERGENCYBRAKE TO A ROSPARAM

9d725d8

debug minor typo bug

fdd0ca6

change the USE_EMERGENCY BRAKE to rosparam, minor typo bug fix

56af55b

Change the Assisted drive code to a ROS package

b62220a

adding ir_drive_controller to the executable

6ed6c3a

do recent changes to ir_drive_controller.py

ce256c3

adding simulation model and launch file

711cf49

add setup.py

ff433c3

Merge branch 'an_ToF' of https://github.com/Roboy/esp-wheelchair into…

5875bcf

… an_ToF

adding assisted_Navigation package

5dd50b4

update readme.md

697b6c7

update reamdme.md

61ad4b7

update readme.md

37db21b

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Integrate ToF Sensor, Implement emergency Brake, and repellent field #4

Integrate ToF Sensor, Implement emergency Brake, and repellent field #4

ronggurmahendra commented Feb 8, 2023 •

edited

Loading

missxa left a comment •

edited

Loading

missxa Feb 14, 2023

missxa Feb 14, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023

missxa Feb 14, 2023

ronggurmahendra Feb 17, 2023


		documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software
		## Variables :


		rospy.init_node('assisted_Navigation')

		assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10)

Integrate ToF Sensor, Implement emergency Brake, and repellent field #4

Are you sure you want to change the base?

Integrate ToF Sensor, Implement emergency Brake, and repellent field #4

Conversation

ronggurmahendra commented Feb 8, 2023 • edited Loading

missxa left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ronggurmahendra commented Feb 8, 2023 •

edited

Loading

missxa left a comment •

edited

Loading