Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 90 commits into
base: wifi-with-cytron
Choose a base branch
from

Conversation

ronggurmahendra
Copy link
Collaborator

@ronggurmahendra ronggurmahendra commented Feb 8, 2023

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

@ronggurmahendra ronggurmahendra self-assigned this Feb 8, 2023
@ronggurmahendra ronggurmahendra changed the title Integrate ToF Sensor, IMplement emergency Brake, repellent field Integrate ToF Sensor, Implement emergency Brake, and repellent field Feb 8, 2023
@ronggurmahendra ronggurmahendra marked this pull request as ready for review February 8, 2023 16:06
Copy link
Contributor

@missxa missxa left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


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

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

@@ -0,0 +1,6 @@
class ManualMode:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

@@ -0,0 +1,6 @@
class ManualMode:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

docstrings for classes are missing, please add

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

software/tof.py Outdated
Comment on lines 31 to 34
for i in range(len(points)):
if(min > points[i][2]):
min = points[i][2]
return min
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use numpy methods to find min

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

software/tof.py Outdated
viewer_front.SpinOnce()
viewer_front.RemovePointCloud( b'scene_cloud_front', 0)
# find the nearest point
global minDist_front
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

not global variables are allowed

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

software/tof.py Outdated
Comment on lines 54 to 70
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 link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

software/tof.py Outdated
Comment on lines 76 to 92
# 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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

comments missing

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

software/tof.py Outdated

rospy.init_node('assisted_Navigation')

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

…ive controller, fix bug with topic /roboy/pinky/middleware/espchair/wheels/dist/front&back
…lified version of the middleware which basically just forward the message
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants