From a507572d3e3f45b88d1ee3f107be62e0d157cf45 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Tue, 31 Jan 2023 22:40:41 +0100 Subject: [PATCH 01/83] adding potential field node and modifying twist_to_pwm to sub to potential field node --- software/potential_field_node.cpp | 108 ++++++++++++++++++++++++++++++ software/tof_potential_field.py | 66 ++++++++++++++++++ software/twist_to_pwm.py | 4 +- 3 files changed, 176 insertions(+), 2 deletions(-) create mode 100644 software/potential_field_node.cpp create mode 100644 software/tof_potential_field.py diff --git a/software/potential_field_node.cpp b/software/potential_field_node.cpp new file mode 100644 index 0000000..e89ff5d --- /dev/null +++ b/software/potential_field_node.cpp @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +double speed; +double angular; +double userspeed; +double emergencyStopThreshold = 0.1; + + +void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) +{ + // Convert the PointCloud2 message to a PCL point cloud + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *cloud); + + // Process the point cloud here + int num_points = cloud->points.size(); + float minDist = 99999; + for(int i = 0; i< cloud->points.size();i++){ + float z = cloud->points[0].z; + if(minDist > z){ + minDist = z; + } + + } + if(minDist > 1){ + minDist = 1; + } + if(minDist < 0){ + minDist = 0; + } + + // later it going to use a different ToF sensor for going backward + // if(userspeed < 0){ + // speed = -1 * minDist; + // }else if (userspeed > 0){ + // speed = minDist; + // }else{ + // speed = 0; + // } + + ROS_INFO_STREAM("minDist:"< 0){ + // ROS_INFO_STREAM("Subscriber velocities:"<<" linear="<("/roboy/pinky/middleware/espchair/wheels/speed", 10); + ros::Publisher potential_Field_angular_pub = n.advertise("/roboy/pinky/middleware/espchair/wheels/angular", 10); + + speed = 0.0; + angular = 0.0; + + // Subscribe to the point cloud topic + ros::Subscriber user_input_sub = n.subscribe("/cmd_vel", 100, userInputCallback); + + // subcribe to the point cloud topic + ros::Subscriber pointcloud_sub = n.subscribe("/royale_camera_driver/point_cloud", 1, pointCloudCallback); + + + ros::Rate loop_rate(10); + while (ros::ok()) + { + ROS_INFO_STREAM(speed); + std_msgs::Float64 speed_msg; + speed_msg.data = speed; + potential_Field_speed_pub.publish(speed_msg); + + std_msgs::Float64 angular_msg; + angular_msg.data = angular; + potential_Field_angular_pub.publish(angular_msg); + + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; +} + + + + diff --git a/software/tof_potential_field.py b/software/tof_potential_field.py new file mode 100644 index 0000000..3524d2b --- /dev/null +++ b/software/tof_potential_field.py @@ -0,0 +1,66 @@ +# # Usage: + +# # cd esp-wheelchair +# # python3 software/tof_potential_field.py + +import rospy +from std_msgs.msg import Float64 +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import Twist +import pcl +import pcl_conversions + +outputspeed = 0.0 +outputangular = 0.0 +inputspeed = 0.0 +inputAngular = 0.0 +emergencyStopThreshold = 0.1 + +def point_cloud_callback(msg): + # Convert the PointCloud2 message to a PCL point cloud + cloud = pcl.PointCloud_PointXYZ() + pcl_conversions.from_ros_message(msg, cloud) + + # Process the point cloud here + num_points = len(cloud.points) + minDist = 99999 + for point in cloud.points: + z = point[2] + if minDist > z: + minDist = z + + if minDist > 1: #thresholding maximum speed + minDist = 1 + if minDist < 0: #thresholding minimum speed + minDist = 0 + + rospy.loginfo("minDist: %f", minDist) + + if minDist < emergencyStopThreshold: # emergency stop + speed = 0.0 + +def user_input_callback(msg): + # global userspeed, angular + inputspeed = msg.linear.x + inputAngular = msg.angular.z + +def main(): + rospy.init_node('tof_potential_field') + + potential_field_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/potential_field', 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) + + rate = rospy.Rate(10) + while not rospy.is_shutdown(): + twist = Twist() + twist.linear.x = outputspeed + twist.angular.z = inputAngular + potential_field_pub.publish(twist) + + rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") + rospy.spin() + rate.sleep() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index aa9edae..2984ecb 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -56,7 +56,7 @@ def cb(msg): -sub = rospy.Subscriber("/cmd_vel", Twist, cb) +sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/potential_field", Twist, cb) rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") -rospy.spin() +rospy.spin() \ No newline at end of file From 358cc26379e6ee74af4b639feca9ca75d54f0580 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 3 Feb 2023 01:55:42 +0100 Subject: [PATCH 02/83] adding potential field as consideration --- software/tof_potential_field.py | 132 ++++++++++++++++++++++---------- software/twist_to_pwm.py | 39 ++++++---- 2 files changed, 118 insertions(+), 53 deletions(-) diff --git a/software/tof_potential_field.py b/software/tof_potential_field.py index 3524d2b..e9a0418 100644 --- a/software/tof_potential_field.py +++ b/software/tof_potential_field.py @@ -8,59 +8,111 @@ from sensor_msgs.msg import PointCloud2 from geometry_msgs.msg import Twist import pcl -import pcl_conversions +import ros_numpy +import numpy as np +# import pcl_conversions +# import pcl.pcl_conversions +import pcl.pcl_visualization -outputspeed = 0.0 -outputangular = 0.0 -inputspeed = 0.0 -inputAngular = 0.0 -emergencyStopThreshold = 0.1 +# outputLinear = 0.0 +# outputAngular = 0.0 +# inputLinear = 0.0 +# inputAngular = 0.0 +emergencyStopThreshold = 0.001 +viewer = pcl.pcl_visualization.PCLVisualizering() + +visualDone = False def point_cloud_callback(msg): - # Convert the PointCloud2 message to a PCL point cloud - cloud = pcl.PointCloud_PointXYZ() - pcl_conversions.from_ros_message(msg, cloud) + + # chaneg 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) - # Process the point cloud here - num_points = len(cloud.points) + p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) + viewer.AddPointCloud(p, b'scene_cloud', 0) + viewer.SpinOnce() + viewer.RemovePointCloud( b'scene_cloud', 0) + # find the nearest point minDist = 99999 - for point in cloud.points: - z = point[2] - if minDist > z: - minDist = z - - if minDist > 1: #thresholding maximum speed - minDist = 1 + maxDist = -99999 + # print("np_points length :", len(np_points) / width) + + for i in range(len(np_points)): + if(minDist > np_points[i][2]): + minDist = np_points[i][2] + if(maxDist < np_points[i][2]): + maxDist = np_points[i][2] + # minDist *=10 + rospy.loginfo("max: %f min : %f", maxDist, minDist ) + if minDist > 0.7: #thresholding maximum speed + minDist = 0.7 if minDist < 0: #thresholding minimum speed minDist = 0 + global outputLinear + # rospy.loginfo("width: %f height : %f", width,height ) + if inputLinear < 0: + outputLinear = -1 * minDist + elif inputLinear > 0: + outputLinear = minDist + else : + outputLinear = 0 + + # outputspeed = minDist + # if minDist < emergencyStopThreshold: # emergency stop + # outputspeed = 0.0 + - rospy.loginfo("minDist: %f", minDist) - - if minDist < emergencyStopThreshold: # emergency stop - speed = 0.0 def user_input_callback(msg): - # global userspeed, angular - inputspeed = msg.linear.x + global inputLinear, inputAngular, outputAngular + inputLinear = msg.linear.x inputAngular = msg.angular.z + # print("inputLinear : ", inputLinear, "inputAngular : ", inputAngular) + outputAngular = inputAngular + + twist = Twist() + print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) + twist.linear.x = outputLinear + twist.angular.z = outputAngular + potential_field_pub.publish(twist) + + + + +# def main(): +rospy.init_node('tof_potential_field') + +potential_field_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/potential_field', 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) + + +# sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/potential_field", Twist, cb) + +# rate = rospy.Rate(10) +# while not rospy.is_shutdown(): +# twist = Twist() +# print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) +# twist.linear.x = outputLinear +# twist.angular.z = outputAngular +# potential_field_pub.publish(twist) + +# rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") +# # rospy.spin() +# rate.sleep() -def main(): - rospy.init_node('tof_potential_field') - potential_field_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/potential_field', 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) - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - twist = Twist() - twist.linear.x = outputspeed - twist.angular.z = inputAngular - potential_field_pub.publish(twist) - rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") - rospy.spin() - rate.sleep() +rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") +rospy.spin() +# rate.sleep() -if __name__ == '__main__': - main() \ No newline at end of file +# if __name__ == '__main__': +# main() \ No newline at end of file diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index 2984ecb..be6c3c7 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -5,7 +5,7 @@ # 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,21 +38,34 @@ 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) + # 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 + # 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) + # lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) + # rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) - pub_l.publish(sign(l)*lPwm) - pub_r.publish(sign(r)*rPwm) - else: - rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") + # pub_l.publish(sign(l)*lPwm) + # pub_r.publish(sign(r)*rPwm) + # else: + # rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") + + 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(sign(l)*lPwm) + pub_r.publish(sign(r)*rPwm) From 4bade8e96c2e4209b4ed2c10eb1f1e05bed9ce75 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 3 Feb 2023 02:52:05 +0100 Subject: [PATCH 03/83] disable the dynamic speed and implement only the emergency brake --- software/tof_potential_field.py | 36 ++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/software/tof_potential_field.py b/software/tof_potential_field.py index e9a0418..962b39e 100644 --- a/software/tof_potential_field.py +++ b/software/tof_potential_field.py @@ -18,7 +18,7 @@ # outputAngular = 0.0 # inputLinear = 0.0 # inputAngular = 0.0 -emergencyStopThreshold = 0.001 +emergencyStopThreshold = 0.01 viewer = pcl.pcl_visualization.PCLVisualizering() visualDone = False @@ -39,6 +39,7 @@ def point_cloud_callback(msg): viewer.SpinOnce() viewer.RemovePointCloud( b'scene_cloud', 0) # find the nearest point + global minDist, maxDist minDist = 99999 maxDist = -99999 # print("np_points length :", len(np_points) / width) @@ -50,22 +51,23 @@ def point_cloud_callback(msg): maxDist = np_points[i][2] # minDist *=10 rospy.loginfo("max: %f min : %f", maxDist, minDist ) - if minDist > 0.7: #thresholding maximum speed - minDist = 0.7 - if minDist < 0: #thresholding minimum speed - minDist = 0 - global outputLinear + + # if minDist > 0.7: #thresholding maximum speed + # minDist = 0.7 + # if minDist < 0: #thresholding minimum speed + # minDist = 0 + # global outputLinear # rospy.loginfo("width: %f height : %f", width,height ) - if inputLinear < 0: - outputLinear = -1 * minDist - elif inputLinear > 0: - outputLinear = minDist - else : - outputLinear = 0 + # if inputLinear < 0: + # outputLinear = -1 * minDist + # elif inputLinear > 0: + # outputLinear = minDist + # else : + # outputLinear = 0 # outputspeed = minDist - # if minDist < emergencyStopThreshold: # emergency stop - # outputspeed = 0.0 + if minDist < emergencyStopThreshold: # emergency stop + outputspeed = 0.0 @@ -75,7 +77,13 @@ def user_input_callback(msg): inputAngular = msg.angular.z # print("inputLinear : ", inputLinear, "inputAngular : ", inputAngular) outputAngular = inputAngular + outputLinear = inputLinear + if(minDist < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF + outputLinear = 0 + elif (minDist < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF + outputLinear = 0 + twist = Twist() print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) twist.linear.x = outputLinear From a61cc991cbd9582caee3b371f6445d28254ffecc Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 3 Feb 2023 09:00:23 +0100 Subject: [PATCH 04/83] cahnge the emergencyStopThreshold --- software/tof_potential_field.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/software/tof_potential_field.py b/software/tof_potential_field.py index 962b39e..25b9c00 100644 --- a/software/tof_potential_field.py +++ b/software/tof_potential_field.py @@ -18,7 +18,7 @@ # outputAngular = 0.0 # inputLinear = 0.0 # inputAngular = 0.0 -emergencyStopThreshold = 0.01 +emergencyStopThreshold = 0.1 viewer = pcl.pcl_visualization.PCLVisualizering() visualDone = False @@ -79,9 +79,11 @@ def user_input_callback(msg): outputAngular = inputAngular outputLinear = inputLinear if(minDist < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF + rospy.loginfo ("ABOUT TO COLLIDE EMERGENCY BRAKE") outputLinear = 0 elif (minDist < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF + rospy.loginfo ("ABOUT TO COLLIDE EMERGENCY BRAKE") outputLinear = 0 twist = Twist() From fe9a3e8f6c1adcd41f735279c2bd030f4dca4396 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 3 Feb 2023 15:40:31 +0100 Subject: [PATCH 05/83] refactor and remove unnecesarry code --- software/tof_potential_field.py | 68 +++++---------------------------- software/twist_to_pwm.py | 16 -------- 2 files changed, 9 insertions(+), 75 deletions(-) diff --git a/software/tof_potential_field.py b/software/tof_potential_field.py index 25b9c00..ac4615f 100644 --- a/software/tof_potential_field.py +++ b/software/tof_potential_field.py @@ -1,7 +1,7 @@ -# # Usage: +# Usage: -# # cd esp-wheelchair -# # python3 software/tof_potential_field.py +# cd esp-wheelchair +# python3 software/tof_potential_field.py import rospy from std_msgs.msg import Float64 @@ -10,14 +10,8 @@ import pcl import ros_numpy import numpy as np -# import pcl_conversions -# import pcl.pcl_conversions import pcl.pcl_visualization -# outputLinear = 0.0 -# outputAngular = 0.0 -# inputLinear = 0.0 -# inputAngular = 0.0 emergencyStopThreshold = 0.1 viewer = pcl.pcl_visualization.PCLVisualizering() @@ -25,7 +19,7 @@ def point_cloud_callback(msg): - # chaneg from pointcloud2 to numpy + # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) height = pc.shape[0] width = pc.shape[1] @@ -41,49 +35,27 @@ def point_cloud_callback(msg): # find the nearest point global minDist, maxDist minDist = 99999 - maxDist = -99999 - # print("np_points length :", len(np_points) / width) - + for i in range(len(np_points)): if(minDist > np_points[i][2]): minDist = np_points[i][2] - if(maxDist < np_points[i][2]): - maxDist = np_points[i][2] - # minDist *=10 - rospy.loginfo("max: %f min : %f", maxDist, minDist ) - # if minDist > 0.7: #thresholding maximum speed - # minDist = 0.7 - # if minDist < 0: #thresholding minimum speed - # minDist = 0 - # global outputLinear - # rospy.loginfo("width: %f height : %f", width,height ) - # if inputLinear < 0: - # outputLinear = -1 * minDist - # elif inputLinear > 0: - # outputLinear = minDist - # else : - # outputLinear = 0 - - # outputspeed = minDist + rospy.loginfo("min : %f", minDist ) if minDist < emergencyStopThreshold: # emergency stop outputspeed = 0.0 - - def user_input_callback(msg): global inputLinear, inputAngular, outputAngular inputLinear = msg.linear.x inputAngular = msg.angular.z - # print("inputLinear : ", inputLinear, "inputAngular : ", inputAngular) outputAngular = inputAngular outputLinear = inputLinear if(minDist < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF - rospy.loginfo ("ABOUT TO COLLIDE EMERGENCY BRAKE") + rospy.loginfo ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 elif (minDist < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF - rospy.loginfo ("ABOUT TO COLLIDE EMERGENCY BRAKE") + rospy.loginfo ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 twist = Twist() @@ -96,33 +68,11 @@ def user_input_callback(msg): # def main(): -rospy.init_node('tof_potential_field') +rospy.init_node('tof_emergency_brake') potential_field_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/potential_field', 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) - -# sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/potential_field", Twist, cb) - -# rate = rospy.Rate(10) -# while not rospy.is_shutdown(): -# twist = Twist() -# print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) -# twist.linear.x = outputLinear -# twist.angular.z = outputAngular -# potential_field_pub.publish(twist) - -# rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") -# # rospy.spin() -# rate.sleep() - - - - rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") rospy.spin() -# rate.sleep() - -# if __name__ == '__main__': -# main() \ No newline at end of file diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index be6c3c7..3f208ba 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -38,22 +38,6 @@ 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) - - # pub_l.publish(sign(l)*lPwm) - # pub_r.publish(sign(r)*rPwm) - # else: - # rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") - 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) From d2a9d445f77e5633c822f19f0d421c219f635bd6 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 3 Feb 2023 15:56:43 +0100 Subject: [PATCH 06/83] update README.md --- software/README.md | 117 ++++++++++++++++++ ...ential_field.py => tof_emergency_brake.py} | 4 +- 2 files changed, 119 insertions(+), 2 deletions(-) rename software/{tof_potential_field.py => tof_emergency_brake.py} (97%) diff --git a/software/README.md b/software/README.md index 8b13789..261995b 100644 --- a/software/README.md +++ b/software/README.md @@ -1 +1,118 @@ +# ToF Setup +## 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 diff --git a/software/tof_potential_field.py b/software/tof_emergency_brake.py similarity index 97% rename from software/tof_potential_field.py rename to software/tof_emergency_brake.py index ac4615f..7613ba8 100644 --- a/software/tof_potential_field.py +++ b/software/tof_emergency_brake.py @@ -1,7 +1,7 @@ # Usage: # cd esp-wheelchair -# python3 software/tof_potential_field.py +# python3 software/tof_emergency_brake.py import rospy from std_msgs.msg import Float64 @@ -33,7 +33,7 @@ def point_cloud_callback(msg): viewer.SpinOnce() viewer.RemovePointCloud( b'scene_cloud', 0) # find the nearest point - global minDist, maxDist + global minDist minDist = 99999 for i in range(len(np_points)): From 1d44b5abaab2a0cb69ae9066f1210083a7a4d636 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 6 Feb 2023 01:13:19 +0100 Subject: [PATCH 07/83] update README.md --- software/README.md | 17 +++++++---------- software/tof_emergency_brake.py | 6 +++--- software/twist_to_pwm.py | 2 +- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/software/README.md b/software/README.md index 261995b..39824c6 100644 --- a/software/README.md +++ b/software/README.md @@ -1,5 +1,6 @@ # 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 @@ -28,9 +29,7 @@ to setup tof1 do these command : cd etc nano rc.local -and then add rc.local with these commands - -... +and then add rc.local with these commands: #stops royale viewer entity systemctl stop royaleviewer @@ -54,18 +53,16 @@ to setup tof2 do these command : cd etc nano rc.local -and then add rc.local with these commands - -... +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” +#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}” +#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” @@ -77,10 +74,10 @@ Reset the ToF sensor then ToF should publish data to /tof2/... Do these command on the ROS master device: -# if you use other ip for ros master then do “export ROS_HOSTNAME={ROS_MASTER_IP}” +#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” +#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 diff --git a/software/tof_emergency_brake.py b/software/tof_emergency_brake.py index 7613ba8..edabbc4 100644 --- a/software/tof_emergency_brake.py +++ b/software/tof_emergency_brake.py @@ -62,7 +62,7 @@ def user_input_callback(msg): print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) twist.linear.x = outputLinear twist.angular.z = outputAngular - potential_field_pub.publish(twist) + emergency_brake_pub.publish(twist) @@ -70,9 +70,9 @@ def user_input_callback(msg): # def main(): rospy.init_node('tof_emergency_brake') -potential_field_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/potential_field', Twist, queue_size=10) +emergency_brake_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/emergency_brake', 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) -rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/potential_field. Spinning...") +rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/emergency_brake. Spinning...") rospy.spin() diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index 3f208ba..cef6964 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -53,7 +53,7 @@ def cb(msg): -sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/potential_field", Twist, cb) +sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/emergency_brake", Twist, cb) rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") rospy.spin() \ No newline at end of file From d8f340bf237a049bef7ffdda77981b5817e3bef4 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 6 Feb 2023 02:47:25 +0100 Subject: [PATCH 08/83] changes so that it consider current speed --- software/tof_emergency_brake.py | 61 +++++++++++++++++++++++++-------- 1 file changed, 47 insertions(+), 14 deletions(-) diff --git a/software/tof_emergency_brake.py b/software/tof_emergency_brake.py index edabbc4..8d31bfa 100644 --- a/software/tof_emergency_brake.py +++ b/software/tof_emergency_brake.py @@ -13,12 +13,39 @@ import pcl.pcl_visualization emergencyStopThreshold = 0.1 -viewer = pcl.pcl_visualization.PCLVisualizering() +viewer_front = pcl.pcl_visualization.PCLVisualizering() + +viewer_back = pcl.pcl_visualization.PCLVisualizering() visualDone = False -def point_cloud_callback(msg): +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) + + p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) + viewer_front.AddPointCloud(p, b'scene_cloud', 0) + viewer_front.SpinOnce() + viewer_front.RemovePointCloud( b'scene_cloud', 0) + # find the nearest point + global minDist_front + minDist_front = 99999 + + for i in range(len(np_points)): + if(minDist_front > np_points[i][2]): + minDist_front = np_points[i][2] + + rospy.loginfo("min front: %f", minDist_front ) + if minDist_front < emergencyStopThreshold and inputLinear > 0: # emergency stop + outputspeed = 0.0 +def point_cloud_back_callback(msg): # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) height = pc.shape[0] @@ -29,32 +56,34 @@ def point_cloud_callback(msg): np_points[:, 2] = np.resize(pc['z'], height * width) p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) - viewer.AddPointCloud(p, b'scene_cloud', 0) - viewer.SpinOnce() - viewer.RemovePointCloud( b'scene_cloud', 0) + viewer_back.AddPointCloud(p, b'scene_cloud', 0) + viewer_back.SpinOnce() + viewer_back.RemovePointCloud( b'scene_cloud', 0) # find the nearest point - global minDist - minDist = 99999 + global minDist_back + minDist_back = 99999 for i in range(len(np_points)): - if(minDist > np_points[i][2]): - minDist = np_points[i][2] + if(minDist_back > np_points[i][2]): + minDist_back = np_points[i][2] - rospy.loginfo("min : %f", minDist ) - if minDist < emergencyStopThreshold: # emergency stop + rospy.loginfo("min back: %f", minDist_back ) + if minDist_back < emergencyStopThreshold and inputLinear < 0: # emergency stop outputspeed = 0.0 + + def user_input_callback(msg): global inputLinear, inputAngular, outputAngular inputLinear = msg.linear.x inputAngular = msg.angular.z outputAngular = inputAngular outputLinear = inputLinear - if(minDist < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF + if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF rospy.loginfo ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif (minDist < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF + elif (minDist_back < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF rospy.loginfo ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 @@ -72,7 +101,11 @@ def user_input_callback(msg): emergency_brake_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/emergency_brake', 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_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) + rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/emergency_brake. Spinning...") rospy.spin() From 5c86e50aaf64dd2ad02267c551058ac80f2ea214 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 6 Feb 2023 15:43:17 +0100 Subject: [PATCH 09/83] deleting a unused file --- software/potential_field_node.cpp | 108 ------------------------------ software/twist_to_pwm.py | 1 - 2 files changed, 109 deletions(-) delete mode 100644 software/potential_field_node.cpp diff --git a/software/potential_field_node.cpp b/software/potential_field_node.cpp deleted file mode 100644 index e89ff5d..0000000 --- a/software/potential_field_node.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -double speed; -double angular; -double userspeed; -double emergencyStopThreshold = 0.1; - - -void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) -{ - // Convert the PointCloud2 message to a PCL point cloud - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - - // Process the point cloud here - int num_points = cloud->points.size(); - float minDist = 99999; - for(int i = 0; i< cloud->points.size();i++){ - float z = cloud->points[0].z; - if(minDist > z){ - minDist = z; - } - - } - if(minDist > 1){ - minDist = 1; - } - if(minDist < 0){ - minDist = 0; - } - - // later it going to use a different ToF sensor for going backward - // if(userspeed < 0){ - // speed = -1 * minDist; - // }else if (userspeed > 0){ - // speed = minDist; - // }else{ - // speed = 0; - // } - - ROS_INFO_STREAM("minDist:"< 0){ - // ROS_INFO_STREAM("Subscriber velocities:"<<" linear="<("/roboy/pinky/middleware/espchair/wheels/speed", 10); - ros::Publisher potential_Field_angular_pub = n.advertise("/roboy/pinky/middleware/espchair/wheels/angular", 10); - - speed = 0.0; - angular = 0.0; - - // Subscribe to the point cloud topic - ros::Subscriber user_input_sub = n.subscribe("/cmd_vel", 100, userInputCallback); - - // subcribe to the point cloud topic - ros::Subscriber pointcloud_sub = n.subscribe("/royale_camera_driver/point_cloud", 1, pointCloudCallback); - - - ros::Rate loop_rate(10); - while (ros::ok()) - { - ROS_INFO_STREAM(speed); - std_msgs::Float64 speed_msg; - speed_msg.data = speed; - potential_Field_speed_pub.publish(speed_msg); - - std_msgs::Float64 angular_msg; - angular_msg.data = angular; - potential_Field_angular_pub.publish(angular_msg); - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} - - - - diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index cef6964..33f0ae9 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -1,6 +1,5 @@ # Usage: - # rosrun rosserial_python serial_node.py tcp # plugin wheelchair power From 3a75ac3d4e8155524aee64937547146ab82ec11e Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 6 Feb 2023 16:57:32 +0100 Subject: [PATCH 10/83] implement manual mode --- software/assistedNavigation.bash | 8 ++ software/manual.py | 6 ++ software/test/tof_test.py | 16 ++++ software/{tof_emergency_brake.py => tof.py} | 81 ++++++++++----------- 4 files changed, 67 insertions(+), 44 deletions(-) create mode 100644 software/assistedNavigation.bash create mode 100644 software/manual.py create mode 100644 software/test/tof_test.py rename software/{tof_emergency_brake.py => tof.py} (72%) diff --git a/software/assistedNavigation.bash b/software/assistedNavigation.bash new file mode 100644 index 0000000..b2a00d2 --- /dev/null +++ b/software/assistedNavigation.bash @@ -0,0 +1,8 @@ +# 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 & +python3 software/twist_to_pwm.py & +python3 software/tof.py & +rosrun teleop_twist_keyboard teleop_twist_keyboard.py \ No newline at end of file diff --git a/software/manual.py b/software/manual.py new file mode 100644 index 0000000..22c3e56 --- /dev/null +++ b/software/manual.py @@ -0,0 +1,6 @@ +class ManualMode: + def __init__(self): + return + + def control(inputLinear, inputAngular): + return (inputLinear,inputAngular) \ No newline at end of file diff --git a/software/test/tof_test.py b/software/test/tof_test.py new file mode 100644 index 0000000..3ebc470 --- /dev/null +++ b/software/test/tof_test.py @@ -0,0 +1,16 @@ +import unittest +import rospy + +class ToF_Test(unittest.TestCase): + def test_string(self): + a = 'some' + b = 'some' + self.assertEqual(a, b) + + def test_boolean(self): + a = True + b = True + self.assertEqual(a, b) + +if __name__ == '__main__': + unittest.main() diff --git a/software/tof_emergency_brake.py b/software/tof.py similarity index 72% rename from software/tof_emergency_brake.py rename to software/tof.py index 8d31bfa..c3f8694 100644 --- a/software/tof_emergency_brake.py +++ b/software/tof.py @@ -1,7 +1,7 @@ # Usage: # cd esp-wheelchair -# python3 software/tof_emergency_brake.py +# python3 software/tof.py import rospy from std_msgs.msg import Float64 @@ -11,14 +11,21 @@ import ros_numpy import numpy as np import pcl.pcl_visualization +from manual import * +useVisual = False emergencyStopThreshold = 0.1 + viewer_front = pcl.pcl_visualization.PCLVisualizering() - viewer_back = pcl.pcl_visualization.PCLVisualizering() - -visualDone = False +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) @@ -33,18 +40,11 @@ def point_cloud_front_callback(msg): viewer_front.AddPointCloud(p, b'scene_cloud', 0) viewer_front.SpinOnce() viewer_front.RemovePointCloud( b'scene_cloud', 0) + # find the nearest point global minDist_front - minDist_front = 99999 + minDist_front = getNearestDistance(np_points) - for i in range(len(np_points)): - if(minDist_front > np_points[i][2]): - minDist_front = np_points[i][2] - - rospy.loginfo("min front: %f", minDist_front ) - if minDist_front < emergencyStopThreshold and inputLinear > 0: # emergency stop - outputspeed = 0.0 - def point_cloud_back_callback(msg): # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) @@ -61,24 +61,31 @@ def point_cloud_back_callback(msg): viewer_back.RemovePointCloud( b'scene_cloud', 0) # find the nearest point global minDist_back - minDist_back = 99999 - - for i in range(len(np_points)): - if(minDist_back > np_points[i][2]): - minDist_back = np_points[i][2] - - rospy.loginfo("min back: %f", minDist_back ) - if minDist_back < emergencyStopThreshold and inputLinear < 0: # emergency stop - outputspeed = 0.0 - - + minDist_back = getNearestDistance(np_points) def user_input_callback(msg): - global inputLinear, inputAngular, outputAngular + global inputLinear, inputAngular inputLinear = msg.linear.x inputAngular = msg.angular.z outputAngular = inputAngular outputLinear = inputLinear + +# def main(): +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) + +rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + +manualMode = ManualMode() + +rate = rospy.Rate(10) # 10hz +while not rospy.is_shutdown(): if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF rospy.loginfo ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 @@ -87,25 +94,11 @@ def user_input_callback(msg): rospy.loginfo ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 + + outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) + rospy.loginfo("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) + twist = Twist() - print("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) twist.linear.x = outputLinear twist.angular.z = outputAngular - emergency_brake_pub.publish(twist) - - - - -# def main(): -rospy.init_node('tof_emergency_brake') - -emergency_brake_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/emergency_brake', 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) - - -rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/emergency_brake. Spinning...") -rospy.spin() + assisted_navigation_pub.publish(twist) From a182f4eef3c147521c3500566cf4cca2dc8b5f3e Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 17:03:29 +0100 Subject: [PATCH 11/83] fix bug viewer --- software/tof.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/software/tof.py b/software/tof.py index c3f8694..c56d010 100644 --- a/software/tof.py +++ b/software/tof.py @@ -37,10 +37,14 @@ def point_cloud_front_callback(msg): np_points[:, 2] = np.resize(pc['z'], height * width) p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) - viewer_front.AddPointCloud(p, b'scene_cloud', 0) + viewer_front.AddPointCloud(p, b'scene_cloud_front', 0) viewer_front.SpinOnce() +<<<<<<< Updated upstream:software/tof.py viewer_front.RemovePointCloud( b'scene_cloud', 0) +======= + viewer_front.RemovePointCloud( b'scene_cloud_front', 0) +>>>>>>> Stashed changes:software/tof_emergency_brake.py # find the nearest point global minDist_front minDist_front = getNearestDistance(np_points) @@ -56,9 +60,9 @@ def point_cloud_back_callback(msg): np_points[:, 2] = np.resize(pc['z'], height * width) p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) - viewer_back.AddPointCloud(p, b'scene_cloud', 0) + viewer_back.AddPointCloud(p, b'scene_cloud_back', 0) viewer_back.SpinOnce() - viewer_back.RemovePointCloud( b'scene_cloud', 0) + viewer_back.RemovePointCloud( b'scene_cloud_back', 0) # find the nearest point global minDist_back minDist_back = getNearestDistance(np_points) From 5bf59943d6a9922eaa4800f81f62252b11f076d3 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 17:05:50 +0100 Subject: [PATCH 12/83] fix bug at viewer --- software/assistedNavigation.bash | 0 software/tof.py | 5 ----- 2 files changed, 5 deletions(-) mode change 100644 => 100755 software/assistedNavigation.bash diff --git a/software/assistedNavigation.bash b/software/assistedNavigation.bash old mode 100644 new mode 100755 diff --git a/software/tof.py b/software/tof.py index c56d010..d0b863a 100644 --- a/software/tof.py +++ b/software/tof.py @@ -39,12 +39,7 @@ def point_cloud_front_callback(msg): p = pcl.PointCloud(np.array(np_points, dtype=np.float32)) viewer_front.AddPointCloud(p, b'scene_cloud_front', 0) viewer_front.SpinOnce() -<<<<<<< Updated upstream:software/tof.py - viewer_front.RemovePointCloud( b'scene_cloud', 0) - -======= viewer_front.RemovePointCloud( b'scene_cloud_front', 0) ->>>>>>> Stashed changes:software/tof_emergency_brake.py # find the nearest point global minDist_front minDist_front = getNearestDistance(np_points) From adbe7305f6829f3864df1bdf506109fbe28c022f Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 17:50:58 +0100 Subject: [PATCH 13/83] refactor --- software/__pycache__/manual.cpython-38.pyc | Bin 0 -> 569 bytes software/assistedNavigation.bash | 5 +- software/manual.py | 4 +- software/tof.py | 55 ++++++++++----------- software/twist_to_pwm.py | 8 +-- 5 files changed, 34 insertions(+), 38 deletions(-) create mode 100644 software/__pycache__/manual.cpython-38.pyc diff --git a/software/__pycache__/manual.cpython-38.pyc b/software/__pycache__/manual.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..30b7e2ea81597eaecb4fc2cd3f9ce9ebb029ed70 GIT binary patch literal 569 zcmZutu};G<5Ve!Egwif$i~)8CbWa2kbOU(x5+b}AiTo7a z|Ck5a7+}wUBuS_si6^M=HYD+dF9Ps^=)i2Z=}ow*c`?yKrWD#+!x(W3umO-DGqR=A zEs1CoSXs;s3Ul;z8E3kbvC(R=s7=W;DGZOLU0$p*DT_4YxrwcwS1WF0T)K^;<#Q7< zmaDvC3>8}>LSIhzibicc#a*pojJ-};2*H}Jy*Zr1;Hy&<`35yNC%IbI)kCf%H%;Fq zH)>HA-0anx4t5@;T2)3Dn6yfU2TFd*)2M^06)z=YO^>lsi@LzL&)5?ja#t~3RC0{; id!=23|C{!>?yw;ZefKhrsPq4gG~v~5(>2b10?0S|V|9lB literal 0 HcmV?d00001 diff --git a/software/assistedNavigation.bash b/software/assistedNavigation.bash index b2a00d2..696057e 100755 --- a/software/assistedNavigation.bash +++ b/software/assistedNavigation.bash @@ -1,7 +1,6 @@ -# if you use other ip for ros master then do “export ROS_HOSTNAME={ROS_MASTER_IP}” -export ROS_HOSTNAME=192.168.0.124 +#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 +#export ROS_MASTER_URI=http://192.168.0.124:11311 roscore & python3 software/twist_to_pwm.py & python3 software/tof.py & diff --git a/software/manual.py b/software/manual.py index 22c3e56..865b096 100644 --- a/software/manual.py +++ b/software/manual.py @@ -2,5 +2,5 @@ class ManualMode: def __init__(self): return - def control(inputLinear, inputAngular): - return (inputLinear,inputAngular) \ No newline at end of file + def control(self, inputLinear, inputAngular): + return inputLinear,inputAngular \ No newline at end of file diff --git a/software/tof.py b/software/tof.py index d0b863a..ce1d3f3 100644 --- a/software/tof.py +++ b/software/tof.py @@ -14,10 +14,16 @@ from manual import * useVisual = False -emergencyStopThreshold = 0.1 -viewer_front = pcl.pcl_visualization.PCLVisualizering() -viewer_back = pcl.pcl_visualization.PCLVisualizering() +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() def getNearestDistance(points): min = 9999 @@ -63,13 +69,24 @@ def point_cloud_back_callback(msg): minDist_back = getNearestDistance(np_points) def user_input_callback(msg): - global inputLinear, inputAngular + inputLinear = msg.linear.x inputAngular = msg.angular.z - outputAngular = inputAngular - outputLinear = inputLinear + # print(inputLinear, inputAngular) + # print("inputLinear : ", inputLinear, ",inputAngular : ", inputAngular) + outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) + # print("outputLinear : ", outputLinear, ", outputAngular : ", outputAngular) + 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 + twist = Twist() + twist.linear.x = outputLinear + twist.angular.z = outputAngular + assisted_navigation_pub.publish(twist) -# def main(): rospy.init_node('assisted_Navigation') assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10) @@ -79,25 +96,5 @@ def user_input_callback(msg): 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) -rospy.loginfo("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") - -manualMode = ManualMode() - -rate = rospy.Rate(10) # 10hz -while not rospy.is_shutdown(): - if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF - rospy.loginfo ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - - elif (minDist_back < emergencyStopThreshold and inputLinear < 0): # asume that this is the back ToF - rospy.loginfo ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") - outputLinear = 0 - - - outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) - rospy.loginfo("outputLinear : ", outputLinear, "outputAngular : ", outputAngular) - - twist = Twist() - twist.linear.x = outputLinear - twist.angular.z = outputAngular - assisted_navigation_pub.publish(twist) +print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") +rospy.spin() \ No newline at end of file diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index 33f0ae9..2edd758 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -46,13 +46,13 @@ def cb(msg): 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) +InputTopic = "/roboy/pinky/middleware/espchair/wheels/assisted_navigation" +sub = rospy.Subscriber(InputTopic, Twist, cb) -sub = rospy.Subscriber("/roboy/pinky/middleware/espchair/wheels/emergency_brake", Twist, cb) - -rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.loginfo("Subscribed to " + InputTopic + ", will publish wheelchair PWM. Spinning...") rospy.spin() \ No newline at end of file From 942cc71d2e436b64bac4633e7021a586e53af0c2 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 18:03:43 +0100 Subject: [PATCH 14/83] adding repelent field --- software/repelent_field.py | 11 +++++++++++ software/tof.py | 34 +++++++++++++++++++--------------- 2 files changed, 30 insertions(+), 15 deletions(-) create mode 100644 software/repelent_field.py diff --git a/software/repelent_field.py b/software/repelent_field.py new file mode 100644 index 0000000..3a17e54 --- /dev/null +++ b/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 = minDistBack + + return outputLinear,inputAngular \ No newline at end of file diff --git a/software/tof.py b/software/tof.py index ce1d3f3..8a8adff 100644 --- a/software/tof.py +++ b/software/tof.py @@ -24,6 +24,7 @@ inputLinear = None inputAngular = None manualMode = ManualMode() +repelentMode = RepelentMode() def getNearestDistance(points): min = 9999 @@ -41,11 +42,11 @@ def point_cloud_front_callback(msg): 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) - - 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) + 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) @@ -59,29 +60,32 @@ def point_cloud_back_callback(msg): 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) - - 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) + 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): - +def user_input_callback(msg): inputLinear = msg.linear.x inputAngular = msg.angular.z - # print(inputLinear, inputAngular) - # print("inputLinear : ", inputLinear, ",inputAngular : ", inputAngular) + +# manual mode outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) - # print("outputLinear : ", outputLinear, ", outputAngular : ", outputAngular) + 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(minDist_front,minDist_back) + twist = Twist() twist.linear.x = outputLinear twist.angular.z = outputAngular From e5efc7a621c2c43926a7f2dca686318b52bdda1a Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 18:04:49 +0100 Subject: [PATCH 15/83] implement repelent field --- software/tof.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/software/tof.py b/software/tof.py index 8a8adff..b0aae5e 100644 --- a/software/tof.py +++ b/software/tof.py @@ -84,7 +84,7 @@ def user_input_callback(msg): outputLinear = 0 # reppelent field - # outputLinear,outputAngular = repelentMode.control(minDist_front,minDist_back) + # outputLinear,outputAngular = repelentMode.control(inputLinear,inputAngular,minDist_front,minDist_back) twist = Twist() twist.linear.x = outputLinear From e8764617a66c533dded0488fdffa02ec44004419 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 6 Feb 2023 18:21:44 +0100 Subject: [PATCH 16/83] editting tof_tes --- software/__pycache__/manual.cpython-38.pyc | Bin 569 -> 569 bytes .../__pycache__/repelent_field.cpython-38.pyc | Bin 0 -> 661 bytes software/test/tof_test.py | 23 +++++++++++------- software/tof.py | 2 +- 4 files changed, 15 insertions(+), 10 deletions(-) create mode 100644 software/__pycache__/repelent_field.cpython-38.pyc diff --git a/software/__pycache__/manual.cpython-38.pyc b/software/__pycache__/manual.cpython-38.pyc index 30b7e2ea81597eaecb4fc2cd3f9ce9ebb029ed70..115e2a7e181bd845602260ba4ba3a72181cab118 100644 GIT binary patch delta 19 ZcmdnVvXg}?l$V!_0SM#_Hgf4R0RSd+12_Nx delta 19 ZcmdnVvXg}?l$V!_0SMf*H*)DS0RSgU16u$9 diff --git a/software/__pycache__/repelent_field.cpython-38.pyc b/software/__pycache__/repelent_field.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1016d1931f179a4f73c2db7136421f5d12749b21 GIT binary patch literal 661 zcmZuuJ5R$f5VrFuQ5zT#EQpD%1MR?ycncQjK*f^9vSPQ5nmENygg{|}Kf_=0hM1Vy zn7A`76p53*?|k;Xe0Q_D+9n{wz1Q@R67q(|viM*e0n9qk1rbEhXTXz}5aA6;*H7X8 zk9c4m0n9d#BncHH@dOp#Gf8~mivV;WLWphI_*%|nE>(3^3YkLvAHg%?6kq}%K_G$- zUVv<1WIobZ$kFFPe^M5*uS+!^*Sg>nDKzg(GuyqJNSUV-p6R|RN7WtI;5M(xMw!e- zZ+34Z#xj*vjA2THMCilja&NtbM;hYmTw0*f+cmiZhlUC|4fI;$KZYuH7SUTx@EA5> z0tDW}#-E?f9S7YI=b(5Y8MAH1ic-`$o@2&tYo33< oXiTUv;ZjAMfh(&QYjZ1$Xym(_L6<55+0p^*J^Z)<= literal 0 HcmV?d00001 diff --git a/software/test/tof_test.py b/software/test/tof_test.py index 3ebc470..e3dff44 100644 --- a/software/test/tof_test.py +++ b/software/test/tof_test.py @@ -1,16 +1,21 @@ import unittest import rospy -class ToF_Test(unittest.TestCase): - def test_string(self): - a = 'some' - b = 'some' - self.assertEqual(a, b) +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 * - def test_boolean(self): - a = True - b = True - self.assertEqual(a, b) + +class Assisted_Navigation_Test(unittest.TestCase): + def test_Repelent_Field(self): + repelentMode = RepelentMode() + repelentMode.control() + def test_Manual_Mode(self): if __name__ == '__main__': unittest.main() diff --git a/software/tof.py b/software/tof.py index b0aae5e..a7cb4b3 100644 --- a/software/tof.py +++ b/software/tof.py @@ -12,7 +12,7 @@ import numpy as np import pcl.pcl_visualization from manual import * - +from repelent_field import * useVisual = False emergencyStopThreshold = 0.1 From 9bdb4adadbce5a96d0d58d6886802110c512d72c Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 6 Feb 2023 18:27:01 +0100 Subject: [PATCH 17/83] adding unit test --- software/test/tof_test.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/software/test/tof_test.py b/software/test/tof_test.py index e3dff44..6559404 100644 --- a/software/test/tof_test.py +++ b/software/test/tof_test.py @@ -14,8 +14,18 @@ class Assisted_Navigation_Test(unittest.TestCase): def test_Repelent_Field(self): repelentMode = RepelentMode() - repelentMode.control() + inputLinear = 10 + inputAngular = 11 + minDistFront = 0.01 + minDistBack = 10 + + assert( repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack), (0, inputAngular)) + def test_Manual_Mode(self): + manualMode = ManualMode() + inputLinear = 10 + inputAngular = 11 + assert(manualMode.control(inputLinear,inputAngular), (inputLinear, inputAngular)) if __name__ == '__main__': unittest.main() From 197b61da8e808eb1e306f672ae80ab3a86f2bc9e Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 8 Feb 2023 00:54:17 +0100 Subject: [PATCH 18/83] adding test file --- software/test/tof_test.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/software/test/tof_test.py b/software/test/tof_test.py index 6559404..905e8e7 100644 --- a/software/test/tof_test.py +++ b/software/test/tof_test.py @@ -19,7 +19,8 @@ def test_Repelent_Field(self): minDistFront = 0.01 minDistBack = 10 - assert( repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack), (0, inputAngular)) + # test + assert(repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack), (0, inputAngular)) def test_Manual_Mode(self): manualMode = ManualMode() From 5c101553f75b88ecdbaae51f26349fd8e440acda Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Wed, 8 Feb 2023 02:17:11 +0100 Subject: [PATCH 19/83] add test file cases --- .../__pycache__/repelent_field.cpython-38.pyc | Bin 661 -> 688 bytes software/repelent_field.py | 4 +-- software/test/tof_test.py | 31 +++++++++++++++--- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/software/__pycache__/repelent_field.cpython-38.pyc b/software/__pycache__/repelent_field.cpython-38.pyc index 1016d1931f179a4f73c2db7136421f5d12749b21..3415e1456541fb2abf6b6e7e3cd7b3b1466857fe 100644 GIT binary patch delta 137 zcmbQrx`CB9l$V!_0SJD6dz9=vk#{F!z{KM!OnQ0~Zx@)W0mW+=Qy8;Zi}-4oYgzLI zz&wT`xfG@v77>P8)*99trZlEt22EzammpJK{s)4|m5k~#w|H_h^IS5EOWca`^Ga@U cLwQb#$=Okp_b{3r8N 0: + if inputLinear >= 0: outputLinear = minDistFront elif inputLinear < 0: - outputLinear = minDistBack + outputLinear = -1*minDistBack return outputLinear,inputAngular \ No newline at end of file diff --git a/software/test/tof_test.py b/software/test/tof_test.py index 905e8e7..e4bf502 100644 --- a/software/test/tof_test.py +++ b/software/test/tof_test.py @@ -8,8 +8,11 @@ import ros_numpy import numpy as np import pcl.pcl_visualization -from manual import * +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): @@ -19,14 +22,34 @@ def test_Repelent_Field(self): 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 - assert(repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack), (0, inputAngular)) + self.assertEqual((outputLiner, outputAngular), (-0.1, inputAngular)) + def test_Manual_Mode(self): manualMode = ManualMode() inputLinear = 10 - inputAngular = 11 - assert(manualMode.control(inputLinear,inputAngular), (inputLinear, inputAngular)) + 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() From 748cf03fb88ab1374b5c72945ecca52562b03c9c Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 02:45:57 +0100 Subject: [PATCH 20/83] refactor : move file to different directories --- {software => scripts}/assistedNavigation.bash | 0 {software => src}/README.md | 0 {software => src}/manual.py | 0 {software => src}/repelent_field.py | 0 {software => src}/test/tof_test.py | 0 {software => src}/tof.py | 0 {software => src}/twist_to_pwm.py | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename {software => scripts}/assistedNavigation.bash (100%) mode change 100755 => 100644 rename {software => src}/README.md (100%) rename {software => src}/manual.py (100%) rename {software => src}/repelent_field.py (100%) rename {software => src}/test/tof_test.py (100%) rename {software => src}/tof.py (100%) rename {software => src}/twist_to_pwm.py (100%) diff --git a/software/assistedNavigation.bash b/scripts/assistedNavigation.bash old mode 100755 new mode 100644 similarity index 100% rename from software/assistedNavigation.bash rename to scripts/assistedNavigation.bash diff --git a/software/README.md b/src/README.md similarity index 100% rename from software/README.md rename to src/README.md diff --git a/software/manual.py b/src/manual.py similarity index 100% rename from software/manual.py rename to src/manual.py diff --git a/software/repelent_field.py b/src/repelent_field.py similarity index 100% rename from software/repelent_field.py rename to src/repelent_field.py diff --git a/software/test/tof_test.py b/src/test/tof_test.py similarity index 100% rename from software/test/tof_test.py rename to src/test/tof_test.py diff --git a/software/tof.py b/src/tof.py similarity index 100% rename from software/tof.py rename to src/tof.py diff --git a/software/twist_to_pwm.py b/src/twist_to_pwm.py similarity index 100% rename from software/twist_to_pwm.py rename to src/twist_to_pwm.py From ada812e827e5b62c79fcdf61606829212781151e Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:06:40 +0100 Subject: [PATCH 21/83] refactor : update readme.md, delete cache --- software/__pycache__/manual.cpython-38.pyc | Bin 569 -> 0 bytes .../__pycache__/repelent_field.cpython-38.pyc | Bin 688 -> 0 bytes src/README.md | 116 +----------------- 3 files changed, 2 insertions(+), 114 deletions(-) delete mode 100644 software/__pycache__/manual.cpython-38.pyc delete mode 100644 software/__pycache__/repelent_field.cpython-38.pyc diff --git a/software/__pycache__/manual.cpython-38.pyc b/software/__pycache__/manual.cpython-38.pyc deleted file mode 100644 index 115e2a7e181bd845602260ba4ba3a72181cab118..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 569 zcmZutu};G<5Ve!Egwif$MeG@9n3xbMVql?NDwZsk6}wHO#4dKC3e=7M1K-eJ%7*v^ zHYV;|LW{&n@7ZltO!J7$fcgHUJW2Mz(ai zB@t}`D~q{7VUE78;!Kw^Hd-wfwJCWfh2gQZ%ZpVeWszn)H?h_8YQ>F=OSf^fd~QO< za+Oz%p<;_f=*#I|(WtGbxT`gcvDZlpAz0J3H-}Rge07Q<-=OB^Bv;G2ddQXJrs=!n zRxRp+o4tC|!Oo*ptIFsClUB*_K*>*e8g)>$;-zG)=`mJnQ5P8Z8GC|5?kc8>N{*3! iue6Ksf72e<9X6z)?_Q=6b^gDRCcNHly205`0Qm;3t94`m diff --git a/software/__pycache__/repelent_field.cpython-38.pyc b/software/__pycache__/repelent_field.cpython-38.pyc deleted file mode 100644 index 3415e1456541fb2abf6b6e7e3cd7b3b1466857fe..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 688 zcmZuuOG^VW5Ki_{i`|Nd-o&HOgY6#>@r7Qrh~i~0VcDeZ(%o!Lb`fPg>7Vg0<*28g z1wk)5lRmWKz-8qB`yN-xHnpirYv* z;DLU z@4_^u>@bRlVUU-_Jnd&t3I}PHVu)9HihXOleOjkA(7JQKzVO9qV7KMtKxC8&#>_Hf zS Date: Thu, 16 Feb 2023 03:24:28 +0100 Subject: [PATCH 22/83] refactor : add comment to bash file --- scripts/assistedNavigation.bash | 6 ++++-- src/README.md | 3 +++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index 696057e..2daa6eb 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -1,6 +1,8 @@ -#export ROS_HOSTNAME=192.168.0.124 +# 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 +export ROS_MASTER_URI=http://192.168.0.124:11311 roscore & python3 software/twist_to_pwm.py & python3 software/tof.py & diff --git a/src/README.md b/src/README.md index dafb80e..b912263 100644 --- a/src/README.md +++ b/src/README.md @@ -1,3 +1,6 @@ # Assisted Navigation + +## How to Run ToF documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software + From 7e9d54b34d9911e2124acd7ad9c8f314fda41b72 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:26:23 +0100 Subject: [PATCH 23/83] refactor : change file name --- src/{manual.py => manual_control.py} | 0 src/{repelent_field.py => repelent_field_control.py} | 0 src/tof.py | 4 ++-- 3 files changed, 2 insertions(+), 2 deletions(-) rename src/{manual.py => manual_control.py} (100%) rename src/{repelent_field.py => repelent_field_control.py} (100%) diff --git a/src/manual.py b/src/manual_control.py similarity index 100% rename from src/manual.py rename to src/manual_control.py diff --git a/src/repelent_field.py b/src/repelent_field_control.py similarity index 100% rename from src/repelent_field.py rename to src/repelent_field_control.py diff --git a/src/tof.py b/src/tof.py index a7cb4b3..94f2850 100644 --- a/src/tof.py +++ b/src/tof.py @@ -11,8 +11,8 @@ import ros_numpy import numpy as np import pcl.pcl_visualization -from manual import * -from repelent_field import * +from manual_control import * +from repelent_field_control import * useVisual = False emergencyStopThreshold = 0.1 From 73ff7517cd6443e143f7a265bbd00e122b4304aa Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:30:43 +0100 Subject: [PATCH 24/83] dev : change to numpy min --- src/tof.py | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/tof.py b/src/tof.py index 94f2850..95a682c 100644 --- a/src/tof.py +++ b/src/tof.py @@ -26,13 +26,6 @@ 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) @@ -49,7 +42,7 @@ def point_cloud_front_callback(msg): viewer_front.RemovePointCloud( b'scene_cloud_front', 0) # find the nearest point global minDist_front - minDist_front = getNearestDistance(np_points) + minDist_front = np.amin(np_points[:, 2]) def point_cloud_back_callback(msg): # change from pointcloud2 to numpy @@ -67,7 +60,7 @@ def point_cloud_back_callback(msg): viewer_back.RemovePointCloud( b'scene_cloud_back', 0) # find the nearest point global minDist_back - minDist_back = getNearestDistance(np_points) + minDist_back = np.amin(np_points[:, 2]) def user_input_callback(msg): inputLinear = msg.linear.x From 9558472ea1d9fac9c51f1f0f75ad7d07fdc9a32d Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:42:03 +0100 Subject: [PATCH 25/83] refactor : use function at point cloud callback --- src/tof.py | 59 ++++++++++++++++++++++++++---------------------------- 1 file changed, 28 insertions(+), 31 deletions(-) diff --git a/src/tof.py b/src/tof.py index 95a682c..4117b94 100644 --- a/src/tof.py +++ b/src/tof.py @@ -26,49 +26,49 @@ manualMode = ManualMode() repelentMode = RepelentMode() +def pointCloud_to_NumpyArray(point_Cloud): + 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): + if(useVisual): + 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 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) + front_Pointcloud_array = pointCloud_to_NumpyArray(pc) + visualizePointCloud(viewer_front, front_Pointcloud_array) # find the nearest point - global minDist_front - minDist_front = np.amin(np_points[:, 2]) + minDist_front = np.amin(front_Pointcloud_array[:, 2]) 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) + back_Pointcloud_array = pointCloud_to_NumpyArray(pc) + visualizePointCloud(viewer_back, back_Pointcloud_array) # find the nearest point - global minDist_back - minDist_back = np.amin(np_points[:, 2]) + minDist_back = np.amin(back_Pointcloud_array[:, 2]) def user_input_callback(msg): inputLinear = msg.linear.x inputAngular = msg.angular.z -# manual mode + # manual mode outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) + # repelent field + # outputLinear,outputAngular = repelentMode.control(inputLinear,inputAngular,minDist_front,minDist_back) + if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 @@ -76,19 +76,16 @@ def user_input_callback(msg): 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) +# main loop 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) From 7d9e460f73e939cec574ef6ef0c63377e66a0a60 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:50:17 +0100 Subject: [PATCH 26/83] dev : change tof.py from publishing Twist to publishing pwm --- src/tof.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/tof.py b/src/tof.py index 4117b94..64b0f9e 100644 --- a/src/tof.py +++ b/src/tof.py @@ -5,6 +5,7 @@ import rospy from std_msgs.msg import Float64 +from std_msgs.msg import Int16 from sensor_msgs.msg import PointCloud2 from geometry_msgs.msg import Twist import pcl @@ -13,8 +14,15 @@ import pcl.pcl_visualization from manual_control import * from repelent_field_control import * + + + +# Parameters useVisual = False +PWM_MIN = 5 +PWMRANGE = 40 +# variable initialization emergencyStopThreshold = 0.1 if(useVisual): viewer_front = pcl.pcl_visualization.PCLVisualizering() @@ -26,6 +34,11 @@ manualMode = ManualMode() repelentMode = RepelentMode() +sign = lambda a: (a>0) - (a<0) + +def mapPwm(x, out_min, out_max): + return x * (out_max - out_min) + out_min; + def pointCloud_to_NumpyArray(point_Cloud): height = point_Cloud.shape[0] width = point_Cloud.shape[1] @@ -81,10 +94,27 @@ def user_input_callback(msg): twist.angular.z = outputAngular assisted_navigation_pub.publish(twist) + 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) + # main loop rospy.init_node('assisted_Navigation') +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) + 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_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, point_cloud_front_callback) From 3960ee3d8aa17b3cad3d84ef833fd20c5a64b86f Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 03:51:48 +0100 Subject: [PATCH 27/83] dev : change tof.py from publishing Twist to publishing pwm --- src/tof.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/tof.py b/src/tof.py index 64b0f9e..357c09a 100644 --- a/src/tof.py +++ b/src/tof.py @@ -37,7 +37,7 @@ sign = lambda a: (a>0) - (a<0) def mapPwm(x, out_min, out_max): - return x * (out_max - out_min) + out_min; + return x * (out_max - out_min) + out_min; def pointCloud_to_NumpyArray(point_Cloud): height = point_Cloud.shape[0] @@ -95,17 +95,17 @@ def user_input_callback(msg): assisted_navigation_pub.publish(twist) rospy.loginfo_throttle(5, "Publishing pwm..") - x = max(min(outputLinear, 1.0), -1.0) - z = max(min(outputAngular, 1.0), -1.0) + 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 + 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) + 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) # main loop rospy.init_node('assisted_Navigation') From 238624a36b2db3257323a31c171add6f8645aac4 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 16 Feb 2023 13:04:06 +0100 Subject: [PATCH 28/83] fix minor bug with viewer --- src/tof.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/tof.py b/src/tof.py index 357c09a..a5246c0 100644 --- a/src/tof.py +++ b/src/tof.py @@ -18,7 +18,7 @@ # Parameters -useVisual = False +useVisual = True PWM_MIN = 5 PWMRANGE = 40 @@ -49,18 +49,19 @@ def pointCloud_to_NumpyArray(point_Cloud): return np_points def visualizePointCloud(viewer ,point_Cloud): - if(useVisual): - 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) + 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 point_cloud_front_callback(msg): # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) front_Pointcloud_array = pointCloud_to_NumpyArray(pc) - visualizePointCloud(viewer_front, front_Pointcloud_array) + + if(useVisual): + visualizePointCloud(viewer_front, front_Pointcloud_array) # find the nearest point minDist_front = np.amin(front_Pointcloud_array[:, 2]) @@ -68,7 +69,8 @@ def point_cloud_back_callback(msg): # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) back_Pointcloud_array = pointCloud_to_NumpyArray(pc) - visualizePointCloud(viewer_back, back_Pointcloud_array) + if(useVisual): + visualizePointCloud(viewer_back, back_Pointcloud_array) # find the nearest point minDist_back = np.amin(back_Pointcloud_array[:, 2]) From 755654c67dc5c3bcb99ae9dc9cb962345d30c4f7 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Thu, 16 Feb 2023 20:08:47 +0100 Subject: [PATCH 29/83] Debug : ignore NaN in min --- software/__pycache__/steer.cpython-38.pyc | Bin 0 -> 2551 bytes src/__pycache__/manual_control.cpython-38.pyc | Bin 0 -> 572 bytes .../repelent_field_control.cpython-38.pyc | Bin 0 -> 673 bytes src/tof.py | 4 ++-- 4 files changed, 2 insertions(+), 2 deletions(-) create mode 100644 software/__pycache__/steer.cpython-38.pyc create mode 100644 src/__pycache__/manual_control.cpython-38.pyc create mode 100644 src/__pycache__/repelent_field_control.cpython-38.pyc diff --git a/software/__pycache__/steer.cpython-38.pyc b/software/__pycache__/steer.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9bad9171bc1ea6c9b61b055446a44c577d21f9b3 GIT binary patch literal 2551 zcmZuz&2Aev5GHq5tN+Ate$W^}!4_#z0j`X==p`tM22N85NQIzLd)PKute2Fu@h+*5 zlwBF}Dd{oqAOpSRQThtJ7CrbCdT2VNWUUN@!p@SM;mo%)!x{bDZZ{ANFZ?I{>l#9T z+r{SaVeuV2>K7msQJkX!M;P0=mwS=t-u~(xM1fmZqR_poMJ=j3SwC+S&8Ugd4~Pb| z@&?h$YcFb1?>Xv(7oZz;uxUOM8P^B7n9}_|thf3skxc16%SPivAysMxH^&;@L)c{~ z!W01%9QiC*RL&8=@TmU=Mb)Lzs76Ctg>{{_X@fT3;HW`c0O6XcKi7=O9|dJkQoLcp z&h2vmj|zZL^c=mz{dX2{pjbYXA?!Nx@7H@{QLsHJ_-HhhMKWeoCVNaxzC0N-mZ#$+ zlY2@G^$C2}p0XCZlbNZ-amF(p#}EOeJRrzB-ChIgUb0iivTPXk(=8qc>@iH{=p|D3 zDW0Pk%`L@qyztIZ;_HBVuOVjq-W)A_kOR)ScjoC}?)I0`)CWfd{lV$Y-oZ1{>-Pq| z=+V<&^!?**w|ilI(5aYumQH%A2WZXqM6%;dOqC0m3!T|1k|P%PG?R(Wju~(i#Ba&- z;i#!hB!sx*%PQ!L+MJz~F^@tm6RtKKDr+ERTDeG*JUa!If^Wl)vIOA}hhB(lxKX(V zG{mQON|$XONv*3chn-tDI=5N%@bTUd(oj}l;lj8=XPOzGO!d=@Ll&BrTRh^UX`aY3 zT~}!evDr5v#M^;ioIL3_SRoC=M`%M&ei6%MQaY4?CLk~ ztbW!2O>NKw67$AO@cdmwtLOe%b7i6FG^53}c?hVk&zqMxhTgeN+VIt%B??H`dU6cm zY>?F$j2T&{A6exeaXCgH6SE@GLUv1WNLNMli6E)1Iul!e6iI&leRuX^mmIF%2`Bb` zW=Vdj0}_#l;aZIzvrH1na+mdF0$EQIP6_N2J}nNJ1Zk2juZ7@heefryr;-B#5KM+c zNHwy&140(Cndov^9L zOA4X14UBatqz2&9SI$qQpi@w7Jp>V5exE8)4r~iZ#bkD2E6Va-+uLh`WU9rJjvqC7 z%rBdlsVDhJNN{Tr-Gm0fW5HwgYMRkZ=>&@6mhm`LNGLDK%;13u6QySpW~#$nB>Jm; znT{GqYrD~%m513+*(jp}S7RZ+j8BC&;V~Fb zk2)=b6N3+E7TpG7 zhuwAuX{)o1sj{bPUI3k@7Sy(CzT z2a}&zUIpsHWF~iD9^HVdG|7d|vqShhU}sUddi#@HM>o__4n2r>*D-aGb_hNM+SI;T UI^6{~apU?v zxzY2cEDHq@V- z#b8f*T`*#|VHbAZ%*}0jPZ92)QCt(_2x!)JSm3Lgbkj7P^j||(7^T6kTdTOTI#-OY5&-T6NkJNUlNbU?n32=)TOV3=_L literal 0 HcmV?d00001 diff --git a/src/tof.py b/src/tof.py index a5246c0..64fdb95 100644 --- a/src/tof.py +++ b/src/tof.py @@ -63,7 +63,7 @@ def point_cloud_front_callback(msg): if(useVisual): visualizePointCloud(viewer_front, front_Pointcloud_array) # find the nearest point - minDist_front = np.amin(front_Pointcloud_array[:, 2]) + minDist_front = np.nanmin(front_Pointcloud_array[:,2]) def point_cloud_back_callback(msg): # change from pointcloud2 to numpy @@ -72,7 +72,7 @@ def point_cloud_back_callback(msg): if(useVisual): visualizePointCloud(viewer_back, back_Pointcloud_array) # find the nearest point - minDist_back = np.amin(back_Pointcloud_array[:, 2]) + minDist_back = np.nanmin(back_Pointcloud_array[:, 2]) def user_input_callback(msg): inputLinear = msg.linear.x From a4fb1c3c1a20ca813bcc6971cb703759702f8290 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Feb 2023 11:49:18 +0100 Subject: [PATCH 30/83] Refactor : adding comment to the code --- src/manual_control.py | 1 + src/repelent_field_control.py | 18 ++++++++++++--- src/tof.py | 41 +++++++++++++++++++++++++---------- 3 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/manual_control.py b/src/manual_control.py index 865b096..76d88b4 100644 --- a/src/manual_control.py +++ b/src/manual_control.py @@ -1,4 +1,5 @@ class ManualMode: + """ Manual Mode no modification between user input and output """ def __init__(self): return diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index e149b84..57f0e96 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -1,11 +1,23 @@ class RepelentMode: + """ Repelent field mode get slower as the robot get near an obstacle """ + DistFront = 1 + DistBack = 1 def __init__(self): return - def control(self, inputLinear, inputAngular, minDistFront, minDistBack): + def setDistanceFront(self, distance): + """ Set member variable DistFront """ + self.DistFront = distance + + def setDistanceBack(self, distance): + """ Set member variable DistBack """ + self.DistBack = distance + + def control(self, inputLinear, inputAngular): + """ give an output based of the distance to an obstacle """ if inputLinear >= 0: - outputLinear = minDistFront + outputLinear = self.DistFront elif inputLinear < 0: - outputLinear = -1*minDistBack + outputLinear = -1*self.DistBack return outputLinear,inputAngular \ No newline at end of file diff --git a/src/tof.py b/src/tof.py index 64fdb95..e7b7275 100644 --- a/src/tof.py +++ b/src/tof.py @@ -21,7 +21,7 @@ useVisual = True PWM_MIN = 5 PWMRANGE = 40 - +MODE = None # variable initialization emergencyStopThreshold = 0.1 if(useVisual): @@ -37,9 +37,11 @@ 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 pointCloud_to_NumpyArray(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) @@ -49,6 +51,7 @@ def pointCloud_to_NumpyArray(point_Cloud): 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() @@ -56,46 +59,55 @@ def visualizePointCloud(viewer ,point_Cloud): def point_cloud_front_callback(msg): + """ Callback function for front ToF sensor """ + # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) front_Pointcloud_array = pointCloud_to_NumpyArray(pc) - + # visualize if useVisual is TRUE if(useVisual): visualizePointCloud(viewer_front, front_Pointcloud_array) - # find the nearest point + # find the nearest point and store it at minDist_front minDist_front = np.nanmin(front_Pointcloud_array[:,2]) + repelentMode.setDistanceFront(minDist_front) def point_cloud_back_callback(msg): + """ Callback function for back ToF sensor """ + # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) back_Pointcloud_array = pointCloud_to_NumpyArray(pc) + # visualize if useVisual is TRUE if(useVisual): visualizePointCloud(viewer_back, back_Pointcloud_array) - # find the nearest point + # find the nearest point and store it at minDist_back minDist_back = np.nanmin(back_Pointcloud_array[:, 2]) + repelentMode.setDistanceBack(minDist_back) def user_input_callback(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 - # manual mode - outputLinear,outputAngular = manualMode.control(inputLinear,inputAngular) + # call the current MODE control function + outputLinear,outputAngular = MODE.control(inputLinear,inputAngular) - # repelent field - # outputLinear,outputAngular = repelentMode.control(inputLinear,inputAngular,minDist_front,minDist_back) - - if(minDist_front < emergencyStopThreshold and inputLinear > 0): # asume that this is the front ToF + # if the minimum distance is within a certaun threshold then brake + if(minDist_front < emergencyStopThreshold and inputLinear > 0): # 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 + elif (minDist_back < emergencyStopThreshold and inputLinear < 0): # this is the back ToF print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 + # 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) @@ -112,13 +124,20 @@ def user_input_callback(msg): # 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, user_input_callback) +# initialize pointlcloud subscriber for ToF sensor 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) From fe9119f91b5d74fb5285ab9264e3d45ed0a9793a Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Feb 2023 11:50:46 +0100 Subject: [PATCH 31/83] adding emergency brake from rosparam --- src/tof.py | 40 +++++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/src/tof.py b/src/tof.py index e7b7275..3efb275 100644 --- a/src/tof.py +++ b/src/tof.py @@ -101,25 +101,27 @@ def user_input_callback(msg): print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 - # 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) + # check if wheelchair_emergency_stopped is TRUE + if not rospy.get_param('wheelchair_emergency_stopped'): + # 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) # main loop rospy.init_node('assisted_Navigation') From aedd093b55b3c24a163db0b96cc6a80afb12b2b7 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Feb 2023 11:54:12 +0100 Subject: [PATCH 32/83] restore previous version of twist_to_pwm.py --- {src => software}/twist_to_pwm.py | 34 +++++++++++++++++-------------- 1 file changed, 19 insertions(+), 15 deletions(-) rename {src => software}/twist_to_pwm.py (57%) diff --git a/src/twist_to_pwm.py b/software/twist_to_pwm.py similarity index 57% rename from src/twist_to_pwm.py rename to software/twist_to_pwm.py index 2edd758..aa9edae 100644 --- a/src/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -1,10 +1,11 @@ # Usage: + # rosrun rosserial_python serial_node.py tcp # plugin wheelchair power # cd esp-wheelchair -# python3 software/twist_to_pwm.py +# python software/twist_to_pwm.py # start ROS joystick node - atk3 is for logitech joystick # roslaunch teleop_twist_joy teleop.launch joy_config:=atk3 @@ -37,22 +38,25 @@ def mapPwm(x, out_min, out_max): 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) + 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) - l = (msg.linear.x - msg.angular.z) / 2.0 - r = (msg.linear.x + msg.angular.z) / 2.0 + pub_l.publish(sign(l)*lPwm) + pub_r.publish(sign(r)*rPwm) + else: + rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") - 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) -InputTopic = "/roboy/pinky/middleware/espchair/wheels/assisted_navigation" -sub = rospy.Subscriber(InputTopic, Twist, cb) +sub = rospy.Subscriber("/cmd_vel", Twist, cb) -rospy.loginfo("Subscribed to " + InputTopic + ", will publish wheelchair PWM. Spinning...") -rospy.spin() \ No newline at end of file +rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.spin() From 28c50d262445bcd2f2fef311ad134334f5dc8830 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 17 Feb 2023 14:39:19 +0100 Subject: [PATCH 33/83] refactor : remove global variable --- .gitignore | 2 +- src/__pycache__/manual_control.cpython-38.pyc | Bin 572 -> 648 bytes .../repelent_field_control.cpython-38.pyc | Bin 673 -> 1479 bytes src/repelent_field_control.py | 7 ++- src/tof.py | 59 +++++++++--------- 5 files changed, 37 insertions(+), 31 deletions(-) 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/src/__pycache__/manual_control.cpython-38.pyc b/src/__pycache__/manual_control.cpython-38.pyc index a6ba297933f4fa8d695e8544d145f3361cdb26f5..7378f1de5e1469404f84eb454036c673167ad4e9 100644 GIT binary patch delta 221 zcmdnP(!t6b%FD~e00iIb-zVRl$m?IP0OX`HL@}l?L@}i>Mlq)_r7*WJM6sl>q_6^M z))cm222J)`T)v5UrHMJd`6;PY)(Q}g0*I%Om#>hUpOTrDnVeXXnV+YSlv+}rnwqCj zTAW&>keOFdTB4Acm!gngS^{DwGlFb~0w8dQ&@mBOA2c+ zgC^T8F5kqw(!?C!{FKy*r|TG*CVMe;3nCgftMkp;K61Wu_Rfj0Oau8d zZukrQk87z?QP8D zddq+Tzd-qJbr@L3tZDn`Uuzax2PnxqREl*tu#N@1V}sRkPzqp!bHO@WP=GBQi%Sxa0`XCzUsvle|Jn98|!tG;hz?jAu@w!k#8RjJi5( z4dS6{={V~5$9m`u6lkxdlF_sCfl{GA@B-aRwBOPT+|mG*ejKGb4);f2GFQqV3Q{Qx zRFH^ce6p8sjP|*oNBCxyBCye%#Ned!oAN1>cr1w4x_T^*k%plfc9DiNPX}HXcl$a> z(l-b;6>Z#1X=%9(R7%mbW)isQorAR|57)qMM&3>?WuCIn zNbihmN~lYui1YvIXP#kxxkMVTzd7J=<% zl^$c{4V9ExSxWp(#v#qCQl5^zaIsR95@KJ|N!=qw{OkLqmfyvEM%P~F izl`}(UGA9f+Snb^X1(3yX1pmu=DwK2dr6u8ps;`8{a8H! delta 372 zcmZWly-LJD5Z>9JcwBPPLIgps5K+?E36`E%X%G(!(;eA82p5xMlIwDvg{^F7=Og$O zzC+&N8u1NuHU=99=KJ>B`Q82S-&A-%8VwoQx%Zh);3bSt4@eo7fMyBTp!pL^gce#7 zORc6X^4j3eEKP2T`li&z0-S6|(8|i>bL3Qw+@8TLaQh6056X4#%_DXR6Y}bqpaBgF zg<&Jss@Uli!?EU#++}OksD{ruO%(BllG)~)teX&dDD0~oPm|y=D=xFDzQVGo6Tep% zvvko0<*Ht;>g%j9Gwc_mE+`4wt(5Yy%-wob_X98hk9=Ui= 0: diff --git a/src/tof.py b/src/tof.py index 3efb275..ad043d5 100644 --- a/src/tof.py +++ b/src/tof.py @@ -27,8 +27,6 @@ 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() @@ -67,7 +65,7 @@ def point_cloud_front_callback(msg): # visualize if useVisual is TRUE if(useVisual): visualizePointCloud(viewer_front, front_Pointcloud_array) - # find the nearest point and store it at minDist_front + # find the nearest point and store it at repelent Class minDist_front = np.nanmin(front_Pointcloud_array[:,2]) repelentMode.setDistanceFront(minDist_front) @@ -80,7 +78,7 @@ def point_cloud_back_callback(msg): # visualize if useVisual is TRUE if(useVisual): visualizePointCloud(viewer_back, back_Pointcloud_array) - # find the nearest point and store it at minDist_back + # find the nearest point and store it at repelent Class minDist_back = np.nanmin(back_Pointcloud_array[:, 2]) repelentMode.setDistanceBack(minDist_back) @@ -94,40 +92,43 @@ def user_input_callback(msg): outputLinear,outputAngular = MODE.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake - if(minDist_front < emergencyStopThreshold and inputLinear > 0): # this is the front ToF + if(repelentMode.getDistanceFront() < emergencyStopThreshold and inputLinear > 0): # this is the front ToF print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif (minDist_back < emergencyStopThreshold and inputLinear < 0): # this is the back ToF + 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 TRUE - if not rospy.get_param('wheelchair_emergency_stopped'): - # 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) + # 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) # main loop rospy.init_node('assisted_Navigation') # initialize mode with manual mode -MODE *= manualMode +MODE = manualMode # initialize wheels publisher pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left", Int16, queue_size=1) From 61e36d87bab62e1dcb801175fd5f4b471c10c6e9 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 17 Feb 2023 14:42:56 +0100 Subject: [PATCH 34/83] refactor : move test file --- scripts/assistedNavigation.bash | 3 +-- {src/test => test}/tof_test.py | 0 2 files changed, 1 insertion(+), 2 deletions(-) rename {src/test => test}/tof_test.py (100%) diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index 2daa6eb..84c3f71 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -4,6 +4,5 @@ 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 & +python3 ../src/tof.py & rosrun teleop_twist_keyboard teleop_twist_keyboard.py \ No newline at end of file diff --git a/src/test/tof_test.py b/test/tof_test.py similarity index 100% rename from src/test/tof_test.py rename to test/tof_test.py From af7ae5b5a58a782eba5073c3e2d6718669a9915c Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 17 Feb 2023 14:48:03 +0100 Subject: [PATCH 35/83] DEBUG : change the repelentField usage in testfile and change the sys file directory --- test/tof_test.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/test/tof_test.py b/test/tof_test.py index e4bf502..33572b2 100644 --- a/test/tof_test.py +++ b/test/tof_test.py @@ -9,10 +9,10 @@ import numpy as np import pcl.pcl_visualization import sys -sys.path.insert(1, '../esp-wheelchair/software') +sys.path.insert(1, '../esp-wheelchair/src') -from manual import * -from repelent_field import * +from manual_control import * +from repelent_field_control import * class Assisted_Navigation_Test(unittest.TestCase): def test_Repelent_Field(self): @@ -21,8 +21,9 @@ def test_Repelent_Field(self): inputAngular = 11 minDistFront = 0.01 minDistBack = 10 - - outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack) + repelentMode.setDistanceFront(minDistFront) + repelentMode.setDistanceBack(minDistBack) + outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) # test # assert(outputLiner == 0 and outputAngular == outputAngular ) self.assertEqual((outputLiner, outputAngular), (0.01, inputAngular)) @@ -31,8 +32,9 @@ def test_Repelent_Field(self): inputAngular = 2 minDistFront = 2 minDistBack = 0.1 - - outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular, minDistFront, minDistBack) + repelentMode.setDistanceBack(minDistFront) + repelentMode.setDistanceBack(minDistBack) + outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) # test self.assertEqual((outputLiner, outputAngular), (-0.1, inputAngular)) From 84af085d157d53b8535aab32452fd04817107b99 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 17 Feb 2023 14:50:07 +0100 Subject: [PATCH 36/83] refactor : adding comment to tof_test.py --- test/tof_test.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/test/tof_test.py b/test/tof_test.py index 33572b2..c96b975 100644 --- a/test/tof_test.py +++ b/test/tof_test.py @@ -15,7 +15,10 @@ 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 @@ -24,8 +27,7 @@ def test_Repelent_Field(self): repelentMode.setDistanceFront(minDistFront) repelentMode.setDistanceBack(minDistBack) outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) - # test - # assert(outputLiner == 0 and outputAngular == outputAngular ) + # test speed when going forward self.assertEqual((outputLiner, outputAngular), (0.01, inputAngular)) repelentMode = RepelentMode() inputLinear = -1 @@ -35,11 +37,12 @@ def test_Repelent_Field(self): repelentMode.setDistanceBack(minDistFront) repelentMode.setDistanceBack(minDistBack) outputLiner,outputAngular = repelentMode.control(inputLinear,inputAngular) - # test + # 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 From 40b25f294c3b00a3fdebb729344b3437fc7a8380 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 23 Feb 2023 13:34:55 +0100 Subject: [PATCH 37/83] edit bash file to not run twist_to_pwm.py --- scripts/assistedNavigation.bash | 1 - 1 file changed, 1 deletion(-) diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index 2daa6eb..08be743 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -4,6 +4,5 @@ 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 \ No newline at end of file From 051e7a5a8606f920be79822295201629d6872243 Mon Sep 17 00:00:00 2001 From: ronggurmahendra <59502688+ronggurmahendra@users.noreply.github.com> Date: Thu, 23 Feb 2023 15:27:53 +0100 Subject: [PATCH 38/83] Delete src/__pycache__ directory Delete unused cache file --- src/__pycache__/manual_control.cpython-38.pyc | Bin 648 -> 0 bytes .../repelent_field_control.cpython-38.pyc | Bin 1479 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/__pycache__/manual_control.cpython-38.pyc delete mode 100644 src/__pycache__/repelent_field_control.cpython-38.pyc diff --git a/src/__pycache__/manual_control.cpython-38.pyc b/src/__pycache__/manual_control.cpython-38.pyc deleted file mode 100644 index 7378f1de5e1469404f84eb454036c673167ad4e9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 648 zcmY*Wu};G<5Ve!E4FwkX0k0WoW&}bY1{UaoSh85gcH2maUF<{%gv0oZ_K&*pl`yTSuB_6JN9<29RLodbTUE|V zDVow6cB`9vtqp6vcC0kZwqe33X4?i+N#VXeFh`sNd;+A%HF=}cH@3jF zVZN}n&Yd;$dFyJi(8`Hi`{lv?LhDK{MCo$xWL^iSTw2pOTTPY^UCMcB%7$}1&136? zKJKqOn)K@u3;MtuYtI2b1$v@S(F~r`;cPIK#SU$E+o8|P(rDqjogmK4ysd;=H{b?X zIa=8og6N&>Z8%viPqQJqHlo&?cVo`siQ5YE9nSCIBUURT&Xtv%J3P;AA))OJ!dqSc g&>qWDn$R>3bJL85aiczL4_4FTze`VW_ZNWt0^Wg=^8f$< diff --git a/src/__pycache__/repelent_field_control.cpython-38.pyc b/src/__pycache__/repelent_field_control.cpython-38.pyc deleted file mode 100644 index 1854f4ed83c67f86e2371a087258cb4a05eee0b5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1479 zcma)6J8u&~5Z=8z$LCjUqC}$Lv7$2)aYctv5)e>;3nCgftMkp;K61Wu_Rfj0Oau8d zZukrQk87z?QP8D zddq+Tzd-qJbr@L3tZDn`Uuzax2PnxqREl*tu#N@1V}sRkPzqp!bHO@WP=GBQi%Sxa0`XCzUsvle|Jn98|!tG;hz?jAu@w!k#8RjJi5( z4dS6{={V~5$9m`u6lkxdlF_sCfl{GA@B-aRwBOPT+|mG*ejKGb4);f2GFQqV3Q{Qx zRFH^ce6p8sjP|*oNBCxyBCye%#Ned!oAN1>cr1w4x_T^*k%plfc9DiNPX}HXcl$a> z(l-b;6>Z#1X=%9(R7%mbW)isQorAR|57)qMM&3>?WuCIn zNbihmN~lYui1YvIXP#kxxkMVTzd7J=<% zl^$c{4V9ExSxWp(#v#qCQl5^zaIsR95@KJ|N!=qw{OkLqmfyvEM%P~F izl`}(UGA9f+Snb^X1(3yX1pmu=DwK2dr6u8ps;`8{a8H! From 6406555ef2ec237031704e34f6e05b813efbc82d Mon Sep 17 00:00:00 2001 From: ronggurmahendra <59502688+ronggurmahendra@users.noreply.github.com> Date: Thu, 23 Feb 2023 15:28:25 +0100 Subject: [PATCH 39/83] Delete software/__pycache__ directory Delete cache file --- software/__pycache__/steer.cpython-38.pyc | Bin 2551 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 software/__pycache__/steer.cpython-38.pyc diff --git a/software/__pycache__/steer.cpython-38.pyc b/software/__pycache__/steer.cpython-38.pyc deleted file mode 100644 index 9bad9171bc1ea6c9b61b055446a44c577d21f9b3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2551 zcmZuz&2Aev5GHq5tN+Ate$W^}!4_#z0j`X==p`tM22N85NQIzLd)PKute2Fu@h+*5 zlwBF}Dd{oqAOpSRQThtJ7CrbCdT2VNWUUN@!p@SM;mo%)!x{bDZZ{ANFZ?I{>l#9T z+r{SaVeuV2>K7msQJkX!M;P0=mwS=t-u~(xM1fmZqR_poMJ=j3SwC+S&8Ugd4~Pb| z@&?h$YcFb1?>Xv(7oZz;uxUOM8P^B7n9}_|thf3skxc16%SPivAysMxH^&;@L)c{~ z!W01%9QiC*RL&8=@TmU=Mb)Lzs76Ctg>{{_X@fT3;HW`c0O6XcKi7=O9|dJkQoLcp z&h2vmj|zZL^c=mz{dX2{pjbYXA?!Nx@7H@{QLsHJ_-HhhMKWeoCVNaxzC0N-mZ#$+ zlY2@G^$C2}p0XCZlbNZ-amF(p#}EOeJRrzB-ChIgUb0iivTPXk(=8qc>@iH{=p|D3 zDW0Pk%`L@qyztIZ;_HBVuOVjq-W)A_kOR)ScjoC}?)I0`)CWfd{lV$Y-oZ1{>-Pq| z=+V<&^!?**w|ilI(5aYumQH%A2WZXqM6%;dOqC0m3!T|1k|P%PG?R(Wju~(i#Ba&- z;i#!hB!sx*%PQ!L+MJz~F^@tm6RtKKDr+ERTDeG*JUa!If^Wl)vIOA}hhB(lxKX(V zG{mQON|$XONv*3chn-tDI=5N%@bTUd(oj}l;lj8=XPOzGO!d=@Ll&BrTRh^UX`aY3 zT~}!evDr5v#M^;ioIL3_SRoC=M`%M&ei6%MQaY4?CLk~ ztbW!2O>NKw67$AO@cdmwtLOe%b7i6FG^53}c?hVk&zqMxhTgeN+VIt%B??H`dU6cm zY>?F$j2T&{A6exeaXCgH6SE@GLUv1WNLNMli6E)1Iul!e6iI&leRuX^mmIF%2`Bb` zW=Vdj0}_#l;aZIzvrH1na+mdF0$EQIP6_N2J}nNJ1Zk2juZ7@heefryr;-B#5KM+c zNHwy&140(Cndov^9L zOA4X14UBatqz2&9SI$qQpi@w7Jp>V5exE8)4r~iZ#bkD2E6Va-+uLh`WU9rJjvqC7 z%rBdlsVDhJNN{Tr-Gm0fW5HwgYMRkZ=>&@6mhm`LNGLDK%;13u6QySpW~#$nB>Jm; znT{GqYrD~%m513+*(jp}S7RZ+j8BC&;V~Fb zk2)=b6N3+E7TpG7 zhuwAuX{)o1sj{bPUI3k@7Sy(CzT z2a}&zUIpsHWF~iD9^HVdG|7d|vqShhU}sUddi#@HM>o__4n2r>*D-aGb_hNM+SI;T UI^ Date: Wed, 1 Mar 2023 09:18:49 +0100 Subject: [PATCH 40/83] Update README.md --- src/README.md | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/README.md b/src/README.md index 884e74a..a309e2d 100644 --- a/src/README.md +++ b/src/README.md @@ -1,5 +1,24 @@ # Assisted Navigation ## How to Run -ToF documentation can be accsessed here : https://devanthro.atlassian.net/wiki/spaces/WS2223/pages/2747269188/AN+Software +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 execute use +``` +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 the to run the assisted navigation node +``` +# if you havent have any rosmaster running +roscore & + +python3 ../src/tof.py & +rosrun teleop_twist_keyboard teleop_twist_keyboard.py + +``` From 5be0e12b320389d0f184f5e87f1b825f0a925c5c Mon Sep 17 00:00:00 2001 From: ronggurmahendra <59502688+ronggurmahendra@users.noreply.github.com> Date: Wed, 1 Mar 2023 09:22:15 +0100 Subject: [PATCH 41/83] Update README.md --- src/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/README.md b/src/README.md index a309e2d..d21e58d 100644 --- a/src/README.md +++ b/src/README.md @@ -1,5 +1,8 @@ # 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 @@ -15,9 +18,10 @@ rosbag play -l ToFSensor_sample2.bag /royale_camera_driver/pointcloud:=/tof2_dri And the to run the assisted navigation node ``` -# if you havent have any rosmaster running +# 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 From 0110972b330db5a4ac1222da84a88cd145bb7a28 Mon Sep 17 00:00:00 2001 From: ronggurmahendra <59502688+ronggurmahendra@users.noreply.github.com> Date: Wed, 1 Mar 2023 09:22:44 +0100 Subject: [PATCH 42/83] Update README.md --- src/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/README.md b/src/README.md index d21e58d..2f31eb9 100644 --- a/src/README.md +++ b/src/README.md @@ -10,13 +10,13 @@ Or Use Bagfile provided in this [link](https://drive.google.com/drive/folders/15IJ5Bk0Abo6tRJbfVam8PI72EpSFbLXI?usp=sharing) -to execute use +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 the to run the assisted navigation node +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 & From 2adaacce907602469e9db2ea3494297359eee991 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Sun, 5 Mar 2023 15:26:57 +0100 Subject: [PATCH 43/83] refactor : requested PRchanges --- src/tof.py | 105 +++++++++++++++++++++++++++-------------------------- 1 file changed, 53 insertions(+), 52 deletions(-) diff --git a/src/tof.py b/src/tof.py index ad043d5..92c3def 100644 --- a/src/tof.py +++ b/src/tof.py @@ -1,11 +1,12 @@ -# Usage: +""" +Usage: -# cd esp-wheelchair -# python3 software/tof.py +cd esp-wheelchair +python3 software/tof.py +""" import rospy -from std_msgs.msg import Float64 -from std_msgs.msg import Int16 +from std_msgs.msg import Float64, Int16 from sensor_msgs.msg import PointCloud2 from geometry_msgs.msg import Twist import pcl @@ -15,30 +16,30 @@ 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 -# Parameters -useVisual = True -PWM_MIN = 5 -PWMRANGE = 40 -MODE = None # variable initialization -emergencyStopThreshold = 0.1 -if(useVisual): +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; + return x * (out_max - out_min) + out_min -def pointCloud_to_NumpyArray(point_Cloud): +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] @@ -56,46 +57,44 @@ def visualizePointCloud(viewer ,point_Cloud): viewer.RemovePointCloud( b'scene_cloud_front', 0) -def point_cloud_front_callback(msg): +def pointCloudFrontCallback(msg): """ Callback function for front ToF sensor """ - # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) - front_Pointcloud_array = pointCloud_to_NumpyArray(pc) - # visualize if useVisual is TRUE - if(useVisual): + 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 point_cloud_back_callback(msg): +def pointCloudBackCallback(msg): """ Callback function for back ToF sensor """ - # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) - back_Pointcloud_array = pointCloud_to_NumpyArray(pc) - # visualize if useVisual is TRUE - if(useVisual): + 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 user_input_callback(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 """ +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) + # 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 + 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 + 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 @@ -124,25 +123,27 @@ def user_input_callback(msg): pub_l.publish(sign(l)*lPwm) pub_r.publish(sign(r)*rPwm) -# 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, user_input_callback) - -# initialize pointlcloud subscriber for ToF sensor -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() \ No newline at end of file +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() + \ No newline at end of file From 5e99a2dab2fde9b7799311b4930a02039dd31fc3 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 10 Mar 2023 09:24:36 +0100 Subject: [PATCH 44/83] rename file to be more appropriate and refactor callback function --- src/{tof.py => tof_teleop.py} | 35 ++++++++++++++--------------------- 1 file changed, 14 insertions(+), 21 deletions(-) rename src/{tof.py => tof_teleop.py} (84%) diff --git a/src/tof.py b/src/tof_teleop.py similarity index 84% rename from src/tof.py rename to src/tof_teleop.py index 92c3def..79fbfb9 100644 --- a/src/tof.py +++ b/src/tof_teleop.py @@ -2,7 +2,7 @@ Usage: cd esp-wheelchair -python3 software/tof.py +python3 src/tof_teleop.py """ import rospy @@ -56,30 +56,23 @@ def visualizePointCloud(viewer ,point_Cloud): viewer.SpinOnce() viewer.RemovePointCloud( b'scene_cloud_front', 0) - -def pointCloudFrontCallback(msg): +def pointCloudCallback(msg,front): """ Callback function for front ToF sensor """ # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) - front_Pointcloud_array = pointCloudToNumpyArray(pc) + 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) + if(front): + visualizePointCloud(viewer_front, Pointcloud_array) + else: + visualizePointCloud(viewer_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) + minDist = np.nanmin(Pointcloud_array[:,2]) + if(front): + repelentMode.setDistanceFront(minDist) + else: + repelentMode.setDistanceBack(minDist) 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 """ @@ -141,8 +134,8 @@ def userInputCallback(msg): 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) + point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True) + point_cloud__back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() From 88a9f7f4443460294c125420b20f25c2fa4d5999 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 10 Mar 2023 17:21:44 +0100 Subject: [PATCH 45/83] fix the logic --- software/twist_to_pwm.py | 6 ++-- src/repelent_field_control.py | 4 ++- src/{tof_teleop.py => tof.py} | 58 +++++++++++++++++++++-------------- src/user_input_handler.py | 56 +++++++++++++++++++++++++++++++++ 4 files changed, 97 insertions(+), 27 deletions(-) rename src/{tof_teleop.py => tof.py} (69%) create mode 100644 src/user_input_handler.py diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index aa9edae..79088bb 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -38,7 +38,7 @@ def mapPwm(x, out_min, out_max): def cb(msg): - if not rospy.get_param('wheelchair_emergency_stopped'): + if True: 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) @@ -49,8 +49,8 @@ def cb(msg): lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) - pub_l.publish(sign(l)*lPwm) - pub_r.publish(sign(r)*rPwm) + pub_l.publish(int(sign(l)*lPwm)) + pub_r.publish(int(sign(r)*rPwm)) else: rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index fd42f13..d5f6564 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -20,9 +20,11 @@ def getDistanceBack(self): return self.DistBack def control(self, inputLinear, inputAngular): """ give an output based of the distance to an obstacle """ - if inputLinear >= 0: + if inputLinear > 0: outputLinear = self.DistFront elif inputLinear < 0: outputLinear = -1*self.DistBack + else: + outputLinear = 0 return outputLinear,inputAngular \ No newline at end of file diff --git a/src/tof_teleop.py b/src/tof.py similarity index 69% rename from src/tof_teleop.py rename to src/tof.py index 79fbfb9..442ed3c 100644 --- a/src/tof_teleop.py +++ b/src/tof.py @@ -2,7 +2,7 @@ Usage: cd esp-wheelchair -python3 src/tof_teleop.py +python3 src/tof.py """ import rospy @@ -15,14 +15,14 @@ import pcl.pcl_visualization from manual_control import * from repelent_field_control import * +from user_input_handler 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 +PWM_MIN = 0 # PWM minimum value +PWMRANGE = 30 # 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() @@ -31,7 +31,8 @@ inputAngular = None manualMode = ManualMode() repelentMode = RepelentMode() -Mode = manualMode +userInputHandler = UserInputHandler(PWM_MIN, PWMRANGE) +Mode = repelentMode sign = lambda a: (a>0) - (a<0) @@ -74,13 +75,17 @@ def pointCloudCallback(msg,front): else: repelentMode.setDistanceBack(minDist) -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 """ +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 - inputLinear = msg.linear.x - inputAngular = msg.angular.z + userInputHandler.setUserInput(msg,right) - # call the current Mode control function + # call the current Mode control function to get the adjusted output + inputLinear,inputAngular = userInputHandler.getUserInput() + # print("inputLinear,inputAngular : ", inputLinear,inputAngular) outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake @@ -106,34 +111,41 @@ def userInputCallback(msg): rospy.loginfo_throttle(5, "Publishing pwm..") x = max(min(outputLinear, 1.0), -1.0) z = max(min(outputAngular, 1.0), -1.0) + print("x : ", x, ", z : ",z) + # l = userInputHandler.translate((x + z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) + # r = userInputHandler.translate((x - z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) - l = (outputLinear - outputAngular) / 2.0 - r = (outputLinear + outputAngular) / 2.0 + l = userInputHandler.translate((x - z)/2, -1, 1, -30, 30 ) + r = userInputHandler.translate((x + z)/2, -1, 1, -30, 30 ) - 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) + # lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) + # rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) + # print("left : ", l, ", right : ",r) + pub_l.publish(l) + pub_r.publish(r) if __name__ == "__main__": # init main loop rospy.init_node('assisted_Navigation') # initialize mode with manual mode - Mode = manualMode + Mode = repelentMode # 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) + pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/adjusted", Int16, queue_size=1) + pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/adjusted", 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 subscriber for user input + user_input_sub_r = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/right', Int16, userInputCallback, True) + user_input_sub_l = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/left', Int16, userInputCallback, False) + # initialize pointlcloud subscriber for ToF sensor + # point_cloud_front_sub = rospy.Subscriber('/royale_camera_driver/point_cloud', PointCloud2, pointCloudCallback, True) + # point_cloud__back_sub = rospy.Subscriber('/royale_camera_driver/point_cloud', PointCloud2, pointCloudCallback, False) + point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True) point_cloud__back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False) diff --git a/src/user_input_handler.py b/src/user_input_handler.py new file mode 100644 index 0000000..28ba34f --- /dev/null +++ b/src/user_input_handler.py @@ -0,0 +1,56 @@ +from std_msgs.msg import Float64, Int16 +class UserInputHandler: + """ Manual Mode no modification between user input and output """ + 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, rigth): + """ Set the user input, store them at class member and define IsDefine as true """ + if (rigth): + 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.PWM_MIN, self.PWM_MIN + self.PWMRANGE, -1, 1) + # inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + + # inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, self.PWM_MIN, self.PWM_MIN + self.PWMRANGE, -1, 1) + # inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) + + inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, -30, 30, -1, 1) + inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -30, 30, -1, 1) + + return inputLinear, inputAngular + else: + inputLinear = 0 + inputAngular = 0 + return inputLinear, inputAngular + + def control(self, inputLinear, inputAngular): + 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) \ No newline at end of file From f15a9627384c3a644fcd3a1f1bfe4b632a346255 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Mon, 13 Mar 2023 15:01:10 +0100 Subject: [PATCH 46/83] fix logic and refactor --- src/tof.py | 34 ++++++++++++++++++++++------------ src/user_input_handler.py | 8 ++------ 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/tof.py b/src/tof.py index 442ed3c..cadf6a1 100644 --- a/src/tof.py +++ b/src/tof.py @@ -19,8 +19,10 @@ # Parameters USEVISUAL = True # USEVISUAL if true will open a window that show the ToF sensor output -PWM_MIN = 0 # PWM minimum value -PWMRANGE = 30 # PWM range value +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 = 0 # Output PWM range value EMERGENCYSTOPTHRESHOLD = 0.1 # emergency stop threshold roboy will stop if it detect a point below the thereshold # variable initialization @@ -31,10 +33,11 @@ inputAngular = None manualMode = ManualMode() repelentMode = RepelentMode() -userInputHandler = UserInputHandler(PWM_MIN, PWMRANGE) -Mode = repelentMode +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +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""" @@ -57,6 +60,15 @@ def visualizePointCloud(viewer ,point_Cloud): viewer.SpinOnce() viewer.RemovePointCloud( b'scene_cloud_front', 0) +def modeCallBack(msg): + """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" + if(msg.data == 1): + Mode = manualMode + elif(msg.data == 2): + Mode = repelentMode + + + def pointCloudCallback(msg,front): """ Callback function for front ToF sensor """ # change from pointcloud2 to numpy @@ -115,9 +127,9 @@ def userInputCallback(msg, right): # l = userInputHandler.translate((x + z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) # r = userInputHandler.translate((x - z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) - l = userInputHandler.translate((x - z)/2, -1, 1, -30, 30 ) - r = userInputHandler.translate((x + z)/2, -1, 1, -30, 30 ) - + l = sign(x)*OUTPUT_PWM_MIN + sign(x) * abs(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + r = sign(x)*OUTPUT_PWM_MIN + sign(x) * abs(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + # lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) # rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) # print("left : ", l, ", right : ",r) @@ -130,7 +142,8 @@ def userInputCallback(msg, right): # initialize mode with manual mode Mode = repelentMode - + mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) + # initialize wheels publisher pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/adjusted", Int16, queue_size=1) pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/adjusted", Int16, queue_size=1) @@ -143,11 +156,8 @@ def userInputCallback(msg, right): user_input_sub_l = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/left', Int16, userInputCallback, False) # initialize pointlcloud subscriber for ToF sensor - # point_cloud_front_sub = rospy.Subscriber('/royale_camera_driver/point_cloud', PointCloud2, pointCloudCallback, True) - # point_cloud__back_sub = rospy.Subscriber('/royale_camera_driver/point_cloud', PointCloud2, pointCloudCallback, False) - point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True) - point_cloud__back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False) + point_cloud_back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() diff --git a/src/user_input_handler.py b/src/user_input_handler.py index 28ba34f..5800f32 100644 --- a/src/user_input_handler.py +++ b/src/user_input_handler.py @@ -1,4 +1,5 @@ from std_msgs.msg import Float64, Int16 + class UserInputHandler: """ Manual Mode no modification between user input and output """ PWM_MIN = 0 # PWM minimum value @@ -26,15 +27,10 @@ def setUserInput(self, value, rigth): 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.PWM_MIN, self.PWM_MIN + self.PWMRANGE, -1, 1) - # inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) - - # inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, self.PWM_MIN, self.PWM_MIN + self.PWMRANGE, -1, 1) - # inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -self.PWMRANGE, self.PWMRANGE, -1, 1) inputLinear = self.translate((self.rightInput.data + self.leftInput.data) / 2, -30, 30, -1, 1) inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -30, 30, -1, 1) - + return inputLinear, inputAngular else: inputLinear = 0 From 3519c60fef0141039f5f78da39c278cecd598d3f Mon Sep 17 00:00:00 2001 From: ronggurmwp Date: Mon, 13 Mar 2023 17:46:14 +0100 Subject: [PATCH 47/83] bug fix and add some additinal funciton to reppelent field --- src/repelent_field_control.py | 20 +++++++++++++++++ src/tof.py | 42 +++++++++++++++++------------------ src/user_input_handler.py | 3 --- 3 files changed, 40 insertions(+), 25 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index d5f6564..caa2eb4 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -8,6 +8,7 @@ def __init__(self): def setDistanceFront(self, distance): """ Set member variable DistFront """ self.DistFront = distance + def getDistanceFront(self): """ Set member variable DistFront """ return self.DistFront @@ -15,11 +16,14 @@ def getDistanceFront(self): 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 """ + # Linear if inputLinear > 0: outputLinear = self.DistFront elif inputLinear < 0: @@ -27,4 +31,20 @@ def control(self, inputLinear, inputAngular): else: outputLinear = 0 + # Quadratic + # if inputLinear > 0: + # outputLinear = pow(self.DistFront,2) + # elif inputLinear < 0: + # outputLinear = -1*pow(self.DistFront,2) + # else: + # outputLinear = 0 + + # Step + # if inputLinear > 0: + # outputLinear = self.DistFront + # elif inputLinear < 0: + # outputLinear = -1*self.DistBack + # else: + # outputLinear = 0 + return outputLinear,inputAngular \ No newline at end of file diff --git a/src/tof.py b/src/tof.py index cadf6a1..12295af 100644 --- a/src/tof.py +++ b/src/tof.py @@ -12,21 +12,22 @@ import pcl import ros_numpy import numpy as np -import pcl.pcl_visualization from manual_control import * from repelent_field_control import * from user_input_handler import * # Parameters -USEVISUAL = True # USEVISUAL if true will open a window that show the ToF sensor output +USEVISUAL = False # USEVISUAL if true will open a window that show the ToF sensor output 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 = 0 # Output PWM range value -EMERGENCYSTOPTHRESHOLD = 0.1 # emergency stop threshold roboy will stop if it detect a point below the thereshold +USE_EMERGENCYSTOP = 1 # Will use emergency stop +EMERGENCYSTOPTHRESHOLD = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold # variable initialization if(USEVISUAL): + # import pcl.pcl_visualization viewer_front = pcl.pcl_visualization.PCLVisualizering() viewer_back = pcl.pcl_visualization.PCLVisualizering() inputLinear = None @@ -63,11 +64,11 @@ def visualizePointCloud(viewer ,point_Cloud): def modeCallBack(msg): """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" if(msg.data == 1): + print("Changing Mode to Manual") Mode = manualMode elif(msg.data == 2): + print("Changing Mode to Repelent") Mode = repelentMode - - def pointCloudCallback(msg,front): """ Callback function for front ToF sensor """ @@ -113,26 +114,24 @@ def userInputCallback(msg, right): 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) - print("x : ", x, ", z : ",z) - # l = userInputHandler.translate((x + z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) - # r = userInputHandler.translate((x - z)/2, -1, 1, PWM_MIN, PWM_MIN + PWMRANGE ) - - l = sign(x)*OUTPUT_PWM_MIN + sign(x) * abs(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - r = sign(x)*OUTPUT_PWM_MIN + sign(x) * abs(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("linear : ", x, ", angular : ",z) - # lPwm = mapPwm(abs(l), PWM_MIN, PWMRANGE) - # rPwm = mapPwm(abs(r), PWM_MIN, PWMRANGE) + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output + r = userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) + l = userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) # print("left : ", l, ", right : ",r) + pub_l.publish(l) pub_r.publish(r) @@ -140,13 +139,12 @@ def userInputCallback(msg, right): # init main loop rospy.init_node('assisted_Navigation') - # initialize mode with manual mode - Mode = repelentMode + # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) # initialize wheels publisher - pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/adjusted", Int16, queue_size=1) - pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/adjusted", Int16, queue_size=1) + pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/adjusted", Int16, queue_size=10) + pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/adjusted", Int16, queue_size=10) # initialize TWIST publisher for simulation assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10) diff --git a/src/user_input_handler.py b/src/user_input_handler.py index 5800f32..8cb7350 100644 --- a/src/user_input_handler.py +++ b/src/user_input_handler.py @@ -37,9 +37,6 @@ def getUserInput(self): inputAngular = 0 return inputLinear, inputAngular - def control(self, inputLinear, inputAngular): - return inputLinear,inputAngular - def translate(self, value, leftMin, leftMax, rightMin, rightMax): # calculate value range leftRange = leftMax - leftMin From e41d69943246ab4380e4ab22fa94fdbe8fbafa8f Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Mon, 13 Mar 2023 17:50:43 +0100 Subject: [PATCH 48/83] delete unused code and uncomment import library --- src/repelent_field_control.py | 8 -------- src/tof.py | 2 +- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index caa2eb4..7f72a18 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -39,12 +39,4 @@ def control(self, inputLinear, inputAngular): # else: # outputLinear = 0 - # Step - # if inputLinear > 0: - # outputLinear = self.DistFront - # elif inputLinear < 0: - # outputLinear = -1*self.DistBack - # else: - # outputLinear = 0 - return outputLinear,inputAngular \ No newline at end of file diff --git a/src/tof.py b/src/tof.py index 12295af..0286d6e 100644 --- a/src/tof.py +++ b/src/tof.py @@ -27,7 +27,7 @@ # variable initialization if(USEVISUAL): - # import pcl.pcl_visualization + import pcl.pcl_visualization viewer_front = pcl.pcl_visualization.PCLVisualizering() viewer_back = pcl.pcl_visualization.PCLVisualizering() inputLinear = None From d62ac43648d6c05052c9331b21ed65b9b10c1000 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Tue, 14 Mar 2023 16:09:57 +0100 Subject: [PATCH 49/83] make another twist_to_pwm for the branch(skip a few parameter check) and restore unintended changes --- software/README.md | 0 software/twist_to_pwm.py | 6 ++-- src/twist_to_pwm.py | 62 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 65 insertions(+), 3 deletions(-) create mode 100644 software/README.md create mode 100644 src/twist_to_pwm.py diff --git a/software/README.md b/software/README.md new file mode 100644 index 0000000..e69de29 diff --git a/software/twist_to_pwm.py b/software/twist_to_pwm.py index 79088bb..aa9edae 100644 --- a/software/twist_to_pwm.py +++ b/software/twist_to_pwm.py @@ -38,7 +38,7 @@ def mapPwm(x, out_min, out_max): def cb(msg): - if True: + 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) @@ -49,8 +49,8 @@ def cb(msg): 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)) + pub_l.publish(sign(l)*lPwm) + pub_r.publish(sign(r)*rPwm) else: rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") diff --git a/src/twist_to_pwm.py b/src/twist_to_pwm.py new file mode 100644 index 0000000..79088bb --- /dev/null +++ b/src/twist_to_pwm.py @@ -0,0 +1,62 @@ +# 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 = 5 +PWMRANGE = 40 + +rospy.init_node("wheelchair_twist_converter") + +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) + +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): + if True: + 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)) + else: + rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") + + + +sub = rospy.Subscriber("/cmd_vel", Twist, cb) + +rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.spin() From ff45f46f45712fc270bf6feb685449a65fc64133 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Tue, 14 Mar 2023 17:22:10 +0100 Subject: [PATCH 50/83] adding rotation to point cloud based of the installation --- src/tof.py | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/tof.py b/src/tof.py index 0286d6e..b6a8ef9 100644 --- a/src/tof.py +++ b/src/tof.py @@ -12,6 +12,7 @@ import pcl import ros_numpy import numpy as np +import math from manual_control import * from repelent_field_control import * from user_input_handler import * @@ -24,6 +25,8 @@ OUTPUT_PWM_RANGE = 0 # Output PWM range value USE_EMERGENCYSTOP = 1 # Will use emergency stop EMERGENCYSTOPTHRESHOLD = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 # variable initialization if(USEVISUAL): @@ -54,6 +57,17 @@ def pointCloudToNumpyArray(point_Cloud): np_points[:, 2] = np.resize(point_Cloud['z'], height * width) return np_points +def applyYRotation(point_Cloud, theta): + rotated_point_Cloud = np.zeros(point_Cloud.shape) + rot = [ + [ math.cos(theta), 0, math.sin(theta)], + [ 0 , 1, 0 ], + [-math.sin(theta), 0, math.cos(theta)] + ] + for rotated, origin in zip(rotated_point_Cloud, point_Cloud): + rotated = np.dot(rot, origin) + return rotated + def visualizePointCloud(viewer ,point_Cloud): """ visualize a numpy point cloud to a viewer """ p = pcl.PointCloud(np.array(point_Cloud, dtype=np.float32)) @@ -70,19 +84,23 @@ def modeCallBack(msg): print("Changing Mode to Repelent") Mode = repelentMode -def pointCloudCallback(msg,front): +def pointCloudCallback(msg,front, angle): """ Callback function for front ToF sensor """ # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) Pointcloud_array = pointCloudToNumpyArray(pc) + + # To maximize the FOV of the TOF sensor we apply a slight pitch ( -5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix + rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) + # visualize if USEVISUAL is TRUE if(USEVISUAL): if(front): - visualizePointCloud(viewer_front, Pointcloud_array) + visualizePointCloud(viewer_front, rotated_Pointcloud_array) else: - visualizePointCloud(viewer_back, Pointcloud_array) + visualizePointCloud(viewer_back, rotated_Pointcloud_array) # find the nearest point and store it at repelent Class - minDist = np.nanmin(Pointcloud_array[:,2]) + minDist = np.nanmin(rotated_Pointcloud_array[:,2]) if(front): repelentMode.setDistanceFront(minDist) else: @@ -154,8 +172,8 @@ def userInputCallback(msg, right): user_input_sub_l = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/left', Int16, userInputCallback, False) # initialize pointlcloud subscriber for ToF sensor - point_cloud_front_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True) - point_cloud_back_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False) + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True, TOF_1_PITCH) + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False, TOF_2_PITCH) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() From d3d6ab1047391ffd9aaa887891ea12b333577aba Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Tue, 14 Mar 2023 17:46:31 +0100 Subject: [PATCH 51/83] debug rotation matrix function bbut disable it since it use too many resource --- src/tof.py | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/tof.py b/src/tof.py index b6a8ef9..d90bcf8 100644 --- a/src/tof.py +++ b/src/tof.py @@ -18,7 +18,7 @@ from user_input_handler import * # Parameters -USEVISUAL = False # USEVISUAL if true will open a window that show the ToF sensor output +USEVISUAL = True # USEVISUAL if true will open a window that show the ToF sensor output INPUT_PWM_MIN = 0 # Input PWM minimum value INPUT_PWM_RANGE = 30 # Input PWM range value OUTPUT_PWM_MIN = 0 # Output PWM minimum value @@ -58,15 +58,18 @@ def pointCloudToNumpyArray(point_Cloud): return np_points def applyYRotation(point_Cloud, theta): - rotated_point_Cloud = np.zeros(point_Cloud.shape) + """Apply a Y axis rotation matrix to pointcloud """ rot = [ [ math.cos(theta), 0, math.sin(theta)], [ 0 , 1, 0 ], [-math.sin(theta), 0, math.cos(theta)] ] - for rotated, origin in zip(rotated_point_Cloud, point_Cloud): - rotated = np.dot(rot, origin) - return rotated + height = point_Cloud.shape[0] + width = point_Cloud.shape[1] + rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) + for i in range(len(point_Cloud)): + rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) + return rotated_point_Cloud def visualizePointCloud(viewer ,point_Cloud): """ visualize a numpy point cloud to a viewer """ @@ -84,15 +87,21 @@ def modeCallBack(msg): print("Changing Mode to Repelent") Mode = repelentMode -def pointCloudCallback(msg,front, angle): +def pointCloudCallback(msg, args): """ Callback function for front ToF sensor """ + # parse arg consist of boolean front and int angle + + front = args[0] + angle = args[1] + # change from pointcloud2 to numpy pc = ros_numpy.numpify(msg) Pointcloud_array = pointCloudToNumpyArray(pc) # To maximize the FOV of the TOF sensor we apply a slight pitch ( -5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix - rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - + # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine + # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) + rotated_Pointcloud_array = Pointcloud_array # visualize if USEVISUAL is TRUE if(USEVISUAL): if(front): @@ -172,8 +181,8 @@ def userInputCallback(msg, right): user_input_sub_l = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/left', Int16, userInputCallback, False) # initialize pointlcloud subscriber for ToF sensor - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True, TOF_1_PITCH) - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False, TOF_2_PITCH) + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_1_PITCH)) + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_2_PITCH)) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() From 5b666d9fe79a5f63096f273a7737a5852591a38d Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Mar 2023 11:07:35 +0100 Subject: [PATCH 52/83] refactor and adding functions to repelent field --- src/repelent_field_control.py | 41 +++++++++++++------ src/tof.py | 74 +++++++++++++++++++++++++---------- 2 files changed, 82 insertions(+), 33 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 7f72a18..0537512 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -1,7 +1,8 @@ class RepelentMode: """ Repelent field mode get slower as the robot get near an obstacle """ - DistFront = 1 - DistBack = 1 + DistFront = 1 # store nearest distance to front obstacle + DistBack = 1 # store nearest distance to back obstacle + Function = 1 # function defaults to linear def __init__(self): return @@ -21,22 +22,36 @@ def getDistanceBack(self): """ Set member variable DistFront """ return self.DistBack + def setFunction(self, function): + """ set memeber variable function """ + self.Function = function + def control(self, inputLinear, inputAngular): """ give an output based of the distance to an obstacle """ - # Linear + if(function == 1): # Linear + outputLinear, outputAngular = self.linear(inputLinear,inputAngular) + elif(function == 2): # Quadratic + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + else: # if undefined defaults to linear + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + return outputLinear,outputAngular + + def linear(self, inputLinear,inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ if inputLinear > 0: outputLinear = self.DistFront elif inputLinear < 0: outputLinear = -1*self.DistBack else: outputLinear = 0 - - # Quadratic - # if inputLinear > 0: - # outputLinear = pow(self.DistFront,2) - # elif inputLinear < 0: - # outputLinear = -1*pow(self.DistFront,2) - # else: - # outputLinear = 0 - - return outputLinear,inputAngular \ No newline at end of file + return outputLinear, inputAngular + + def quadratic(self, inputLinear, inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ + if inputLinear > 0: + outputLinear = pow(self.DistFront,2) + elif inputLinear < 0: + outputLinear = -1*pow(self.DistFront,2) + else: + outputLinear = 0 + return outputLinear, inputAngular \ No newline at end of file diff --git a/src/tof.py b/src/tof.py index b6a8ef9..0600482 100644 --- a/src/tof.py +++ b/src/tof.py @@ -7,29 +7,45 @@ """ import rospy from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointCloud2, Image from geometry_msgs.msg import Twist -import pcl import ros_numpy + +import pcl import numpy as np import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + from manual_control import * from repelent_field_control import * from user_input_handler import * # Parameters -USEVISUAL = False # USEVISUAL if true will open a window that show the ToF sensor output +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +USE_REAR_CAMERA = True # USE_REAR_CAMERA if true will publish a data of rear camera + +USE_EMERGENCYSTOP = 1 # 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 = 0 # Output PWM range value -USE_EMERGENCYSTOP = 1 # Will use emergency stop -EMERGENCYSTOPTHRESHOLD = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + +THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + TOF_1_PITCH = 11 TOF_2_PITCH = -5 +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left/adjusted" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right/adjusted" +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right" + + # variable initialization -if(USEVISUAL): +if(USE_VISUAL_POINT_CLOUD): import pcl.pcl_visualization viewer_front = pcl.pcl_visualization.PCLVisualizering() viewer_back = pcl.pcl_visualization.PCLVisualizering() @@ -84,6 +100,14 @@ def modeCallBack(msg): print("Changing Mode to Repelent") Mode = repelentMode +def repelentFieldCallBack(msg): + """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ + if(msg.data == 1): + print("Changing Repelelent field to Linear") + + elif(msg.data == 2): + print("Changing Repelelent field to Quadratic") + def pointCloudCallback(msg,front, angle): """ Callback function for front ToF sensor """ # change from pointcloud2 to numpy @@ -93,8 +117,8 @@ def pointCloudCallback(msg,front, angle): # To maximize the FOV of the TOF sensor we apply a slight pitch ( -5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - # visualize if USEVISUAL is TRUE - if(USEVISUAL): + # visualize if USE_VISUAL_POINT_CLOUD is TRUE + if(USE_VISUAL_POINT_CLOUD): if(front): visualizePointCloud(viewer_front, rotated_Pointcloud_array) else: @@ -120,10 +144,10 @@ def userInputCallback(msg, right): 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 + if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP 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 + elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0): # this is the back ToF print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 # Check if wheelchair_emergency_stopped is defined @@ -152,7 +176,8 @@ def userInputCallback(msg, right): pub_l.publish(l) pub_r.publish(r) - +def rearCameraCallback(msg): + return if __name__ == "__main__": # init main loop rospy.init_node('assisted_Navigation') @@ -160,21 +185,30 @@ def userInputCallback(msg, right): # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) + mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) + # initialize wheels publisher - pub_l = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/left/adjusted", Int16, queue_size=10) - pub_r = rospy.Publisher("/roboy/pinky/middleware/espchair/wheels/right/adjusted", Int16, queue_size=10) + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) + pub_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) # initialize TWIST publisher for simulation - assisted_navigation_pub = rospy.Publisher('/roboy/pinky/middleware/espchair/wheels/assisted_navigation', Twist, queue_size=10) + assisted_navigation_pub = rospy.Publisher(ASSISTED_NAVIGATION_TOPIC_OUTPUT, Twist, queue_size=10) # initialize subscriber for user input - user_input_sub_r = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/right', Int16, userInputCallback, True) - user_input_sub_l = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/left', Int16, userInputCallback, False) - - # initialize pointlcloud subscriber for ToF sensor - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, True, TOF_1_PITCH) - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, False, TOF_2_PITCH) + 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 pointlcloud subscriber for ToF sensor args = (front, PitchAngle) + # ToF 1 for the back hence first arg is False + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) + # ToF 2 for the front hence first arg is True + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) + rear_image_pub = rospy.Publisher("rear_image",Image, queue_size=10) + bridge = CvBridge() + rear_image_sub = rospy.Subscriber("/tof1_driver/gray_image",Image,rearCameraCallback) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() \ No newline at end of file From f7e9df55c060db1af47df5f810db65469251c7b8 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Mar 2023 12:09:45 +0100 Subject: [PATCH 53/83] debug --- src/repelent_field_control.py | 5 +++-- src/tof.py | 13 +++++++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 0537512..66c3388 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -24,13 +24,14 @@ def getDistanceBack(self): def setFunction(self, function): """ set memeber variable function """ + print("Changing Repelelent field to ", function) self.Function = function def control(self, inputLinear, inputAngular): """ give an output based of the distance to an obstacle """ - if(function == 1): # Linear + if(self.Function == 1): # Linear outputLinear, outputAngular = self.linear(inputLinear,inputAngular) - elif(function == 2): # Quadratic + elif(self.Function == 2): # Quadratic outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) else: # if undefined defaults to linear outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) diff --git a/src/tof.py b/src/tof.py index f991987..86451b8 100644 --- a/src/tof.py +++ b/src/tof.py @@ -54,7 +54,7 @@ manualMode = ManualMode() repelentMode = RepelentMode() userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -Mode = manualMode +Mode = repelentMode sign = lambda a: (a>0) - (a<0) @@ -96,6 +96,10 @@ def visualizePointCloud(viewer ,point_Cloud): def modeCallBack(msg): """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" + """ + to change the mode do + rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 + """ if(msg.data == 1): print("Changing Mode to Manual") Mode = manualMode @@ -105,11 +109,16 @@ def modeCallBack(msg): def repelentFieldCallBack(msg): """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ + """ + to change the function do + rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 + """ if(msg.data == 1): print("Changing Repelelent field to Linear") - + repelentMode.setFunction(msg.data) elif(msg.data == 2): print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) def pointCloudCallback(msg, args): """ Callback function for front ToF sensor """ From 06e6c31ef463589caf87e23dea7c11c304a8ba1a Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Mar 2023 12:18:20 +0100 Subject: [PATCH 54/83] adding rear camera --- src/image_handler.py | 30 ++++++++++++++++++++++++++++++ src/tof.py | 8 +++----- 2 files changed, 33 insertions(+), 5 deletions(-) create mode 100644 src/image_handler.py diff --git a/src/image_handler.py b/src/image_handler.py new file mode 100644 index 0000000..8c169ce --- /dev/null +++ b/src/image_handler.py @@ -0,0 +1,30 @@ +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError +import rospy +import cv2 + +class ImageHandler: + + def __init__(self, image_sub_topic): + # self.image_pub = rospy.Publisher("image_topic_2",Image) + + self.bridge = CvBridge() + self.image_sub = rospy.Subscriber(image_sub_topic,Image,self.callback) + + def callback(self,data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") + except CvBridgeError as e: + print(e) + + (rows,cols) = cv_image.shape + if cols > 60 and rows > 60 : + cv2.circle(cv_image, (50,50), 10, 255) + + cv2.imshow("Image window", cv_image) + cv2.waitKey(3) + + # try: + # self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) + # except CvBridgeError as e: + # print(e) diff --git a/src/tof.py b/src/tof.py index 86451b8..6867617 100644 --- a/src/tof.py +++ b/src/tof.py @@ -20,7 +20,7 @@ from manual_control import * from repelent_field_control import * from user_input_handler import * - +from image_handler import ImageHandler # Parameters USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output USE_REAR_CAMERA = True # USE_REAR_CAMERA if true will publish a data of rear camera @@ -223,10 +223,8 @@ def rearCameraCallback(msg): # ToF 2 for the front hence first arg is True point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - rear_image_pub = rospy.Publisher("rear_image",Image, queue_size=10) - bridge = CvBridge() - rear_image_sub = rospy.Subscriber("/tof1_driver/gray_image",Image,rearCameraCallback) - + if (USE_REAR_CAMERA): + imageHandler = ImageHandler("/tof1_driver/gray_image") print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() From d0386da057ce34bd4744919ac67f1cc5953e7b8e Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Mar 2023 12:21:35 +0100 Subject: [PATCH 55/83] adding parameter --- src/tof.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/tof.py b/src/tof.py index 6867617..ce36537 100644 --- a/src/tof.py +++ b/src/tof.py @@ -23,9 +23,9 @@ from image_handler import ImageHandler # Parameters USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -USE_REAR_CAMERA = True # USE_REAR_CAMERA if true will publish a data of rear camera +USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera -USE_EMERGENCYSTOP = 1 # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +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 @@ -163,10 +163,10 @@ def userInputCallback(msg, right): outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake - if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0): # this is the front ToF + if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0): # this is the back ToF + elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 # Check if wheelchair_emergency_stopped is defined From 679b87305a9191c9268bb055ca6ba56d55826495 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 17 Mar 2023 12:36:37 +0100 Subject: [PATCH 56/83] changing the quadratic parameter --- src/repelent_field_control.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 66c3388..469e4a7 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -24,8 +24,8 @@ def getDistanceBack(self): def setFunction(self, function): """ set memeber variable function """ - print("Changing Repelelent field to ", function) self.Function = function + print("Changing Repelelent field to ", self.Function) def control(self, inputLinear, inputAngular): """ give an output based of the distance to an obstacle """ @@ -52,7 +52,7 @@ def quadratic(self, inputLinear, inputAngular): if inputLinear > 0: outputLinear = pow(self.DistFront,2) elif inputLinear < 0: - outputLinear = -1*pow(self.DistFront,2) + outputLinear = -pow(self.DistBack,2) else: outputLinear = 0 return outputLinear, inputAngular \ No newline at end of file From b6935e6fd1416029710affca3d44b5e6c33f93df Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Sun, 19 Mar 2023 17:06:15 +0100 Subject: [PATCH 57/83] delete unnessesarry code --- src/image_handler.py | 30 ------------------ src/repelent_field_control.py | 14 ++++++++- src/tof.py | 57 ++++++++++++++++++----------------- src/twist_to_pwm.py | 8 ++--- 4 files changed, 46 insertions(+), 63 deletions(-) delete mode 100644 src/image_handler.py diff --git a/src/image_handler.py b/src/image_handler.py deleted file mode 100644 index 8c169ce..0000000 --- a/src/image_handler.py +++ /dev/null @@ -1,30 +0,0 @@ -from sensor_msgs.msg import Image -from cv_bridge import CvBridge, CvBridgeError -import rospy -import cv2 - -class ImageHandler: - - def __init__(self, image_sub_topic): - # self.image_pub = rospy.Publisher("image_topic_2",Image) - - self.bridge = CvBridge() - self.image_sub = rospy.Subscriber(image_sub_topic,Image,self.callback) - - def callback(self,data): - try: - cv_image = self.bridge.imgmsg_to_cv2(data, "mono8") - except CvBridgeError as e: - print(e) - - (rows,cols) = cv_image.shape - if cols > 60 and rows > 60 : - cv2.circle(cv_image, (50,50), 10, 255) - - cv2.imshow("Image window", cv_image) - cv2.waitKey(3) - - # try: - # self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) - # except CvBridgeError as e: - # print(e) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 469e4a7..d996c15 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -33,6 +33,8 @@ def control(self, inputLinear, inputAngular): outputLinear, outputAngular = self.linear(inputLinear,inputAngular) elif(self.Function == 2): # Quadratic outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + elif(self.Function == 3): # Quadratic + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) else: # if undefined defaults to linear outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) return outputLinear,outputAngular @@ -55,4 +57,14 @@ def quadratic(self, inputLinear, inputAngular): outputLinear = -pow(self.DistBack,2) else: outputLinear = 0 - return outputLinear, inputAngular \ No newline at end of file + return outputLinear, inputAngular + + def quadratic2(self, inputLinear, inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ + if inputLinear > 0: + outputLinear = 2*pow(self.DistFront,2) + elif inputLinear < 0: + outputLinear = -2*pow(self.DistBack,2) + else: + outputLinear = 0 + return outputLinear, inputAngular diff --git a/src/tof.py b/src/tof.py index ce36537..282e9a0 100644 --- a/src/tof.py +++ b/src/tof.py @@ -20,28 +20,29 @@ from manual_control import * from repelent_field_control import * from user_input_handler import * -from image_handler import ImageHandler + # Parameters USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera USE_EMERGENCYSTOP = 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 = 0 # Output PWM range value +OUTPUT_PWM_RANGE = 20 # Output PWM range value THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold TOF_1_PITCH = 11 TOF_2_PITCH = -5 -LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left/adjusted" -RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right/adjusted" +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' -LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left" -RIGHT_MOTOR_TOPIC_INPUT = "/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" # variable initialization @@ -54,11 +55,10 @@ manualMode = ManualMode() repelentMode = RepelentMode() userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -Mode = 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 @@ -119,6 +119,9 @@ def repelentFieldCallBack(msg): elif(msg.data == 2): print("Changing Repelelent field to Quadratic") repelentMode.setFunction(msg.data) + elif(msg.data == 3): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) def pointCloudCallback(msg, args): """ Callback function for front ToF sensor """ @@ -131,7 +134,7 @@ def pointCloudCallback(msg, args): pc = ros_numpy.numpify(msg) Pointcloud_array = pointCloudToNumpyArray(pc) - # To maximize the FOV of the TOF sensor we apply a slight pitch ( -5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix + # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) rotated_Pointcloud_array = Pointcloud_array @@ -159,7 +162,7 @@ def userInputCallback(msg, right): # call the current Mode control function to get the adjusted output inputLinear,inputAngular = userInputHandler.getUserInput() - # print("inputLinear,inputAngular : ", inputLinear,inputAngular) + print("inputLinear,inputAngular : ", inputLinear,inputAngular) outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake @@ -182,21 +185,21 @@ def userInputCallback(msg, right): z = max(min(outputAngular, 1.0), -1.0) print("linear : ", x, ", angular : ",z) - # publish the TWIST output - twist = Twist() - twist.linear.x = x - twist.angular.z = -1*z - assisted_navigation_pub.publish(twist) + if (USE_SIMULATION): + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) # publish the PWM output - r = userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) - l = userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE ) - # print("left : ", l, ", right : ",r) + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("left : ", l, ", right : ",r) pub_l.publish(l) pub_r.publish(r) -def rearCameraCallback(msg): - return + if __name__ == "__main__": # init main loop rospy.init_node('assisted_Navigation') @@ -207,12 +210,13 @@ def rearCameraCallback(msg): mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) - pub_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) - - # initialize TWIST publisher for simulation - assisted_navigation_pub = rospy.Publisher(ASSISTED_NAVIGATION_TOPIC_OUTPUT, Twist, queue_size=10) + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_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) @@ -222,9 +226,6 @@ def rearCameraCallback(msg): point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) # ToF 2 for the front hence first arg is True point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - - if (USE_REAR_CAMERA): - imageHandler = ImageHandler("/tof1_driver/gray_image") print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() diff --git a/src/twist_to_pwm.py b/src/twist_to_pwm.py index 79088bb..70ad2c6 100644 --- a/src/twist_to_pwm.py +++ b/src/twist_to_pwm.py @@ -23,13 +23,13 @@ from geometry_msgs.msg import Twist from std_msgs.msg import Int16 -PWM_MIN = 5 -PWMRANGE = 40 +PWM_MIN = 0 +PWMRANGE = 30 rospy.init_node("wheelchair_twist_converter") -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) +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) From d4f7255fe92567e8de2dc38ef9c532a69c268e10 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Sun, 19 Mar 2023 17:07:57 +0100 Subject: [PATCH 58/83] adding another quadratic function --- src/repelent_field_control.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 469e4a7..6173cf2 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -29,6 +29,7 @@ def setFunction(self, function): def control(self, inputLinear, inputAngular): """ give an output based of the distance to an obstacle """ + print("Minimum Distance Back : ", self.DistBack) if(self.Function == 1): # Linear outputLinear, outputAngular = self.linear(inputLinear,inputAngular) elif(self.Function == 2): # Quadratic @@ -50,9 +51,9 @@ def linear(self, inputLinear,inputAngular): def quadratic(self, inputLinear, inputAngular): """ adjust speed linearly to the distance of nearest obstacle """ if inputLinear > 0: - outputLinear = pow(self.DistFront,2) + outputLinear = 1.5*pow(self.DistFront,2) elif inputLinear < 0: - outputLinear = -pow(self.DistBack,2) + outputLinear = -1.5*pow(self.DistBack,2) else: outputLinear = 0 - return outputLinear, inputAngular \ No newline at end of file + return outputLinear, inputAngular From eabad4e8685b552a70257b8eb4ac0fa4e2bb8b62 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Sun, 19 Mar 2023 17:51:07 +0100 Subject: [PATCH 59/83] split assisted navigation to 2 node --- src/assistedNavigation.py | 174 ++++++++++++++++++++++++++++++++++++++ src/tof.py | 3 +- src/tof_handler.py | 104 +++++++++++++++++++++++ 3 files changed, 280 insertions(+), 1 deletion(-) create mode 100644 src/assistedNavigation.py create mode 100644 src/tof_handler.py diff --git a/src/assistedNavigation.py b/src/assistedNavigation.py new file mode 100644 index 0000000..6030353 --- /dev/null +++ b/src/assistedNavigation.py @@ -0,0 +1,174 @@ +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from manual_control import * +from repelent_field_control import * +from user_input_handler import * + +# Parameters +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera + +USE_EMERGENCYSTOP = 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 + +THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" + +inputLinear = None +inputAngular = None +manualMode = ManualMode() +repelentMode = RepelentMode() +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +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 modeCallBack(msg): + """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" + """ + to change the mode do + rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Mode to Manual") + Mode = manualMode + elif(msg.data == 2): + print("Changing Mode to Repelent") + Mode = repelentMode + +def repelentFieldCallBack(msg): + """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ + """ + to change the function do + rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Repelelent field to Linear") + repelentMode.setFunction(msg.data) + elif(msg.data == 2): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + elif(msg.data == 3): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + +def minDistanceCallback(msg, args): + """ Callback function for front ToF sensor """ + # parse arg consist of boolean front and int angle + front = args[0] + + if(front): + repelentMode.setDistanceFront(msg.data) + else: + repelentMode.setDistanceBack(msg.data) + +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 + inputLinear,inputAngular = userInputHandler.getUserInput() + print("inputLinear,inputAngular : ", inputLinear,inputAngular) + outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) + + # if the minimum distance is within a certaun threshold then brake + if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # 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 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) + + if (USE_SIMULATION): + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("left : ", l, ", right : ",r) + + pub_l.publish(l) + pub_r.publish(r) + +if __name__ == "__main__": + # init main loop + rospy.init_node('AssistedNavigation_main') + + # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode + mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) + + mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) + + # initialize wheels publisher + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_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) + + min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Int16, minDistanceCallback, (True)) + min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Int16, minDistanceCallback, (False)) + + # # initialize pointlcloud subscriber for ToF sensor + # # ToF 1 for the back hence first arg is False + # point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, minDistanceCallback, (False)) + # # ToF 2 for the front hence first arg is True + # point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, minDistanceCallback, (True)) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() + \ No newline at end of file diff --git a/src/tof.py b/src/tof.py index 282e9a0..9aead8c 100644 --- a/src/tof.py +++ b/src/tof.py @@ -43,7 +43,8 @@ ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" - +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" # variable initialization if(USE_VISUAL_POINT_CLOUD): diff --git a/src/tof_handler.py b/src/tof_handler.py new file mode 100644 index 0000000..fb3468c --- /dev/null +++ b/src/tof_handler.py @@ -0,0 +1,104 @@ + +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from manual_control import * +from repelent_field_control import * +from user_input_handler import * + +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" + +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 + +# variable initialization +if(USE_VISUAL_POINT_CLOUD): + import pcl.pcl_visualization + viewer_front = pcl.pcl_visualization.PCLVisualizering() + viewer_back = pcl.pcl_visualization.PCLVisualizering() + + +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 applyYRotation(point_Cloud, theta): + """Apply a Y axis rotation matrix to pointcloud """ + rot = [ + [ math.cos(theta), 0, math.sin(theta)], + [ 0 , 1, 0 ], + [-math.sin(theta), 0, math.cos(theta)] + ] + height = point_Cloud.shape[0] + width = point_Cloud.shape[1] + rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) + for i in range(len(point_Cloud)): + rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) + return rotated_point_Cloud + +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 pointCloudCallback(msg, args): + """ Callback function for front ToF sensor """ + # parse arg consist of boolean front and int angle + + front = args[0] + angle = args[1] + + # change from pointcloud2 to numpy + pc = ros_numpy.numpify(msg) + Pointcloud_array = pointCloudToNumpyArray(pc) + + # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix + # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine + # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) + rotated_Pointcloud_array = Pointcloud_array + + # visualize if USE_VISUAL_POINT_CLOUD is TRUE + if(USE_VISUAL_POINT_CLOUD): + if(front): + visualizePointCloud(viewer_front, rotated_Pointcloud_array) + else: + visualizePointCloud(viewer_back, rotated_Pointcloud_array) + # find the nearest point and store it at repelent Class + minDist = np.nanmin(rotated_Pointcloud_array[:,2]) + +if __name__ == "__main__": + # init main loop + rospy.init_node('AssistedNavigation_tof') + + # initialize pointlcloud subscriber for ToF sensor + # ToF 1 for the back hence first arg is False + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) + # ToF 2 for the front hence first arg is True + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) + + min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Int16, queue_size=3) + min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Int16, queue_size=3) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() + \ No newline at end of file From 8ada88fc5de8f346116d421e581999292b3f2ab6 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Tue, 21 Mar 2023 18:47:13 +0100 Subject: [PATCH 60/83] adding drive mode as a class, refactor assisted navigation iao and drive controller, fix bug with topic /roboy/pinky/middleware/espchair/wheels/dist/front&back --- scripts/assistedNavigation.bash | 11 ++++--- scripts/assistedNavigation_aio.bash | 10 ++++++ src/{tof.py => assistedNavigation_aio.py} | 11 ++++--- ...istedNavigation.py => drive_controller.py} | 32 ++++++------------- src/drive_mode.py | 12 +++++++ src/tof_handler.py | 23 +++++++++---- 6 files changed, 61 insertions(+), 38 deletions(-) create mode 100644 scripts/assistedNavigation_aio.bash rename src/{tof.py => assistedNavigation_aio.py} (97%) rename src/{assistedNavigation.py => drive_controller.py} (85%) create mode 100644 src/drive_mode.py diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index 84c3f71..a863c97 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -1,8 +1,11 @@ # this script run all the necessary setup to run the assisted navigation program using the teleop keyboard -export ROS_HOSTNAME=192.168.0.124 +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.0.124:11311 -roscore & -python3 ../src/tof.py & +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 +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/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/src/tof.py b/src/assistedNavigation_aio.py similarity index 97% rename from src/tof.py rename to src/assistedNavigation_aio.py index 9aead8c..5c96731 100644 --- a/src/tof.py +++ b/src/assistedNavigation_aio.py @@ -2,7 +2,7 @@ Usage: cd esp-wheelchair -python3 src/tof.py +python3 src/assistedNavigation_aio.py """ import rospy @@ -20,6 +20,7 @@ from manual_control import * from repelent_field_control import * from user_input_handler import * +from drive_mode import * # Parameters USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output @@ -56,7 +57,7 @@ manualMode = ManualMode() repelentMode = RepelentMode() userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -Mode = manualMode +mode = DriveMode(manualMode) sign = lambda a: (a>0) - (a<0) @@ -103,10 +104,10 @@ def modeCallBack(msg): """ if(msg.data == 1): print("Changing Mode to Manual") - Mode = manualMode + mode.setMode(manualMode) elif(msg.data == 2): print("Changing Mode to Repelent") - Mode = repelentMode + mode.setMode(repelentMode) def repelentFieldCallBack(msg): """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ @@ -164,7 +165,7 @@ def userInputCallback(msg, right): # call the current Mode control function to get the adjusted output inputLinear,inputAngular = userInputHandler.getUserInput() print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) + outputLinear,outputAngular = mode.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF diff --git a/src/assistedNavigation.py b/src/drive_controller.py similarity index 85% rename from src/assistedNavigation.py rename to src/drive_controller.py index 6030353..6c91654 100644 --- a/src/assistedNavigation.py +++ b/src/drive_controller.py @@ -13,6 +13,7 @@ from manual_control import * from repelent_field_control import * from user_input_handler import * +from drive_mode import * # Parameters USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output @@ -44,7 +45,7 @@ manualMode = ManualMode() repelentMode = RepelentMode() userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -Mode = manualMode +mode = DriveMode(manualMode) sign = lambda a: (a>0) - (a<0) @@ -60,10 +61,10 @@ def modeCallBack(msg): """ if(msg.data == 1): print("Changing Mode to Manual") - Mode = manualMode + mode.setMode(manualMode) elif(msg.data == 2): print("Changing Mode to Repelent") - Mode = repelentMode + mode.setMode(repelentMode) def repelentFieldCallBack(msg): """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ @@ -81,11 +82,10 @@ def repelentFieldCallBack(msg): print("Changing Repelelent field to Quadratic") repelentMode.setFunction(msg.data) -def minDistanceCallback(msg, args): +def minDistanceCallback(msg, front): """ Callback function for front ToF sensor """ - # parse arg consist of boolean front and int angle - front = args[0] - + # subscribe to MinDist topic to feed into repelent Mode + print("ToF distance : ", msg.data) if(front): repelentMode.setDistanceFront(msg.data) else: @@ -102,7 +102,7 @@ def userInputCallback(msg, right): # call the current Mode control function to get the adjusted output inputLinear,inputAngular = userInputHandler.getUserInput() print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = Mode.control(inputLinear,inputAngular) + outputLinear,outputAngular = mode.control(inputLinear,inputAngular) # if the minimum distance is within a certaun threshold then brake if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF @@ -111,12 +111,6 @@ def userInputCallback(msg, right): elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # 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 output to wheels rospy.loginfo_throttle(5, "Publishing pwm..") @@ -160,14 +154,8 @@ def userInputCallback(msg, right): 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) - min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Int16, minDistanceCallback, (True)) - min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Int16, minDistanceCallback, (False)) - - # # initialize pointlcloud subscriber for ToF sensor - # # ToF 1 for the back hence first arg is False - # point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, minDistanceCallback, (False)) - # # ToF 2 for the front hence first arg is True - # point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, minDistanceCallback, (True)) + min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Float64, minDistanceCallback, True) + min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Float64, minDistanceCallback, False) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() diff --git a/src/drive_mode.py b/src/drive_mode.py new file mode 100644 index 0000000..c7635ef --- /dev/null +++ b/src/drive_mode.py @@ -0,0 +1,12 @@ +class DriveMode: + """ this class is a place holder for the current active class """ + def __init__(self,mode): + self.Mode = mode + return + + def setMode(self, mode): + self.Mode = mode + return + + def control(self, inputLinear, inputAngular): + return self.Mode.control(inputLinear,inputAngular) \ No newline at end of file diff --git a/src/tof_handler.py b/src/tof_handler.py index fb3468c..21119f0 100644 --- a/src/tof_handler.py +++ b/src/tof_handler.py @@ -75,16 +75,25 @@ def pointCloudCallback(msg, args): # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - rotated_Pointcloud_array = Pointcloud_array # visualize if USE_VISUAL_POINT_CLOUD is TRUE if(USE_VISUAL_POINT_CLOUD): if(front): - visualizePointCloud(viewer_front, rotated_Pointcloud_array) + visualizePointCloud(viewer_front, Pointcloud_array) else: - visualizePointCloud(viewer_back, rotated_Pointcloud_array) + visualizePointCloud(viewer_back, Pointcloud_array) + + print(Pointcloud_array.shape) # find the nearest point and store it at repelent Class - minDist = np.nanmin(rotated_Pointcloud_array[:,2]) + minDist = np.nanmin(Pointcloud_array[:,2]) + print(minDist) + # publish the minimum distance to the MIN_DIST TOPIC + if(front): + min_dist_front_pub.publish(minDist) + print("Minimum distance front : ", minDist) + else: + min_dist_back_pub.publish(minDist) + print("Minimum distance back : ", minDist) if __name__ == "__main__": # init main loop @@ -96,9 +105,9 @@ def pointCloudCallback(msg, args): # ToF 2 for the front hence first arg is True point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Int16, queue_size=3) - min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Int16, queue_size=3) + min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Float64, queue_size=3) + min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Float64, queue_size=3) - print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + print("publishing to " + MIN_DIST_FRONT_TOPIC + " and " + MIN_DIST_BACK_TOPIC + " Spinning...") rospy.spin() \ No newline at end of file From 638f88761be86d835e764a399c0c59fa5ce8c87e Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 22 Mar 2023 18:50:25 +0100 Subject: [PATCH 61/83] change parameter to use somulation and add simulation to bash file --- scripts/assistedNavigation.bash | 8 +++++++- src/drive_controller.py | 7 +++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index a863c97..29659c3 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -6,6 +6,12 @@ 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 -rosrun teleop_twist_keyboard teleop_twist_keyboard.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 teleop_twist_keyboard teleop_twist_keyboard.py \ No newline at end of file diff --git a/src/drive_controller.py b/src/drive_controller.py index 6c91654..b060596 100644 --- a/src/drive_controller.py +++ b/src/drive_controller.py @@ -20,7 +20,7 @@ USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP -USE_SIMULATION = False +USE_SIMULATION = True INPUT_PWM_MIN = 0 # Input PWM minimum value INPUT_PWM_RANGE = 30 # Input PWM range value @@ -34,9 +34,12 @@ LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" -ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" + +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" From 1b0ef7d4733f18de6586f116a1b56e406ac79629 Mon Sep 17 00:00:00 2001 From: ronggurmwp Date: Wed, 5 Apr 2023 14:42:45 +0200 Subject: [PATCH 62/83] change namefile of ToF implementation and add a IR sensor version --- scripts/assistedNavigation.bash | 4 + src/IR_State.py | 37 ++ src/drive_mode.py | 22 +- src/ir_drive_controller.py | 121 +++++ src/ir_handler.py | 55 +++ src/repelent_field_control.py | 142 +++--- ...n_aio.py => tof_assistedNavigation_aio.py} | 466 +++++++++--------- ..._controller.py => tof_drive_controller.py} | 328 ++++++------ src/tof_handler.py | 224 ++++----- src/twist_to_pwm.py | 124 ++--- 10 files changed, 870 insertions(+), 653 deletions(-) create mode 100644 src/IR_State.py create mode 100644 src/ir_drive_controller.py create mode 100644 src/ir_handler.py rename src/{assistedNavigation_aio.py => tof_assistedNavigation_aio.py} (97%) rename src/{drive_controller.py => tof_drive_controller.py} (97%) diff --git a/scripts/assistedNavigation.bash b/scripts/assistedNavigation.bash index 29659c3..d81d563 100644 --- a/scripts/assistedNavigation.bash +++ b/scripts/assistedNavigation.bash @@ -14,4 +14,8 @@ roslaunch gazebo_ros shapes_world.launch 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/src/IR_State.py b/src/IR_State.py new file mode 100644 index 0000000..ca63630 --- /dev/null +++ b/src/IR_State.py @@ -0,0 +1,37 @@ +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT = 1 + _FRONT_LEFT = 2 + _BACK_RIGHT = 3 + _BACK_LEFT = 4 + def __init__(self): + self.dist_front_right = 0 + self.dist_front_left = 0 + self.dist_back_right = 0 + self.dist_back_left = 0 + return + + def set(self, input, pos): + if pos == IRState._FRONT_RIGHT: + self.dist_front_right = input + elif pos == IRState._FRONT_LEFT: + self.dist_front_left = input + elif pos == IRState._BACK_RIGHT: + self.dist_back_right = input + elif pos == IRState._BACK_LEFT: + self.dist_back_left = input + + def get(self,pos): + if pos == IRState._FRONT_RIGHT: + return self.dist_front_right + elif pos == IRState._FRONT_LEFT: + return self.dist_front_left + elif pos == IRState._BACK_RIGHT: + return self.dist_back_right + elif pos == IRState._BACK_LEFT: + return self.dist_back_left + + + + def control(self, inputLinear, inputAngular): + return inputLinear,inputAngular \ No newline at end of file diff --git a/src/drive_mode.py b/src/drive_mode.py index c7635ef..1831ff6 100644 --- a/src/drive_mode.py +++ b/src/drive_mode.py @@ -1,12 +1,12 @@ -class DriveMode: - """ this class is a place holder for the current active class """ - def __init__(self,mode): - self.Mode = mode - return - - def setMode(self, mode): - self.Mode = mode - return - - def control(self, inputLinear, inputAngular): +class DriveMode: + """ this class is a place holder for the current active class """ + def __init__(self,mode): + self.Mode = mode + return + + def setMode(self, mode): + self.Mode = mode + return + + def control(self, inputLinear, inputAngular): return self.Mode.control(inputLinear,inputAngular) \ No newline at end of file diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py new file mode 100644 index 0000000..b531cfb --- /dev/null +++ b/src/ir_drive_controller.py @@ -0,0 +1,121 @@ +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from user_input_handler import UserInputHandler +from IR_State import IRState +# Parameters +USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_SIMULATION = True + +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 + +THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + +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" + +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +# Set up publishers for each distance topic +DIST_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_right" +DIST_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_left" +DIST_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_right" +DIST_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_left" + +inputLinear = None +inputAngular = None +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 + inputLinear,inputAngular = userInputHandler.getUserInput() + print("inputLinear,inputAngular : ", inputLinear,inputAngular) + + # if the minimum distance is within a certaun threshold then brake + if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + outputLinear = 0 + + outputLinear, outputAngular = inputLinear,inputAngular + # 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) + + if (USE_SIMULATION): + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("left : ", l, ", right : ",r) + + pub_l.publish(l) + pub_r.publish(r) + +def irSensorCallback(msg, pos): + irState.set(pos,msg.data) + return + +if __name__ == "__main__": + # init main loop + rospy.init_node('Ir_AssistedNavigation_main') + + # initialize wheels publisher + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_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 topic + front_right_sub = rospy.Subscriber(DIST_FRONT_RIGHT_TOPIC, Int16, irSensorCallback, 1) + front_left_sub = rospy.Subscriber(DIST_FRONT_LEFT_TOPIC, Int16, irSensorCallback, 2) + back_right_sub = rospy.Subscriber(DIST_BACK_RIGHT_TOPIC, Int16, irSensorCallback, 3) + back_left_sub = rospy.Subscriber(DIST_BACK_LEFT_TOPIC, Int16, irSensorCallback, 4) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() + \ No newline at end of file diff --git a/src/ir_handler.py b/src/ir_handler.py new file mode 100644 index 0000000..ca9203d --- /dev/null +++ b/src/ir_handler.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python + +import rospy +from std_msgs.msg import Int16 + +import Jetson.GPIO as GPIO + +# Set up GPIO pins +pin_front_right = 1 +pin_front_left = 2 +pin_back_right = 3 +pin_back_left = 4 + +GPIO.setmode(GPIO.BOARD) +GPIO.setup(pin_front_right, GPIO.IN) +GPIO.setup(pin_front_left, GPIO.IN) +GPIO.setup(pin_back_right, GPIO.IN) +GPIO.setup(pin_back_left, GPIO.IN) + +# Set up ROS node +rospy.init_node("Ir_handler") + +# Set up publishers for each distance topic +DIST_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_right" +DIST_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_left" +DIST_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_right" +DIST_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_left" + +pub_front_right = rospy.Publisher(DIST_FRONT_RIGHT_TOPIC, Int16, queue_size=10) +pub_front_left = rospy.Publisher(DIST_FRONT_LEFT_TOPIC, Int16, queue_size=10) +pub_back_right = rospy.Publisher(DIST_BACK_RIGHT_TOPIC, Int16, queue_size=10) +pub_back_left = rospy.Publisher(DIST_BACK_LEFT_TOPIC, Int16, queue_size=10) + +# Define callback function for GPIO pin changes +def gpio_callback(channel): + if channel == pin_front_right: + pub_front_right.publish(GPIO.input(channel)) + elif channel == pin_front_left: + pub_front_left.publish(GPIO.input(channel)) + elif channel == pin_back_right: + pub_back_right.publish(GPIO.input(channel)) + elif channel == pin_back_left: + pub_back_left.publish(GPIO.input(channel)) + +# Set up GPIO pin event detection +GPIO.add_event_detect(pin_front_right, GPIO.BOTH, callback=gpio_callback) +GPIO.add_event_detect(pin_front_left, GPIO.BOTH, callback=gpio_callback) +GPIO.add_event_detect(pin_back_right, GPIO.BOTH, callback=gpio_callback) +GPIO.add_event_detect(pin_back_left, GPIO.BOTH, callback=gpio_callback) + +# Spin ROS node +rospy.spin() + +# Clean up GPIO pins +GPIO.cleanup() diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py index 43a2d38..87e6941 100644 --- a/src/repelent_field_control.py +++ b/src/repelent_field_control.py @@ -1,71 +1,71 @@ -class RepelentMode: - """ Repelent field mode get slower as the robot get near an obstacle """ - DistFront = 1 # store nearest distance to front obstacle - DistBack = 1 # store nearest distance to back obstacle - Function = 1 # function defaults to linear - 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 setFunction(self, function): - """ set memeber variable function """ - self.Function = function - print("Changing Repelelent field to ", self.Function) - - def control(self, inputLinear, inputAngular): - """ give an output based of the distance to an obstacle """ - print("Minimum Distance Back : ", self.DistBack) - if(self.Function == 1): # Linear - outputLinear, outputAngular = self.linear(inputLinear,inputAngular) - elif(self.Function == 2): # Quadratic - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - elif(self.Function == 3): # Quadratic - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - else: # if undefined defaults to linear - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - return outputLinear,outputAngular - - def linear(self, inputLinear,inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = self.DistFront - elif inputLinear < 0: - outputLinear = -1*self.DistBack - else: - outputLinear = 0 - return outputLinear, inputAngular - - def quadratic(self, inputLinear, inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = 1.5*pow(self.DistFront,2) - elif inputLinear < 0: - outputLinear = -1.5*pow(self.DistBack,2) - else: - outputLinear = 0 - return outputLinear, inputAngular - - def quadratic2(self, inputLinear, inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = 2*pow(self.DistFront,2) - elif inputLinear < 0: - outputLinear = -2*pow(self.DistBack,2) - else: - outputLinear = 0 - return outputLinear, inputAngular +class RepelentMode: + """ Repelent field mode get slower as the robot get near an obstacle """ + DistFront = 1 # store nearest distance to front obstacle + DistBack = 1 # store nearest distance to back obstacle + Function = 1 # function defaults to linear + 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 setFunction(self, function): + """ set memeber variable function """ + self.Function = function + print("Changing Repelelent field to ", self.Function) + + def control(self, inputLinear, inputAngular): + """ give an output based of the distance to an obstacle """ + print("Minimum Distance Back : ", self.DistBack) + if(self.Function == 1): # Linear + outputLinear, outputAngular = self.linear(inputLinear,inputAngular) + elif(self.Function == 2): # Quadratic + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + elif(self.Function == 3): # Quadratic + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + else: # if undefined defaults to linear + outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) + return outputLinear,outputAngular + + def linear(self, inputLinear,inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ + if inputLinear > 0: + outputLinear = self.DistFront + elif inputLinear < 0: + outputLinear = -1*self.DistBack + else: + outputLinear = 0 + return outputLinear, inputAngular + + def quadratic(self, inputLinear, inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ + if inputLinear > 0: + outputLinear = 1.5*pow(self.DistFront,2) + elif inputLinear < 0: + outputLinear = -1.5*pow(self.DistBack,2) + else: + outputLinear = 0 + return outputLinear, inputAngular + + def quadratic2(self, inputLinear, inputAngular): + """ adjust speed linearly to the distance of nearest obstacle """ + if inputLinear > 0: + outputLinear = 2*pow(self.DistFront,2) + elif inputLinear < 0: + outputLinear = -2*pow(self.DistBack,2) + else: + outputLinear = 0 + return outputLinear, inputAngular diff --git a/src/assistedNavigation_aio.py b/src/tof_assistedNavigation_aio.py similarity index 97% rename from src/assistedNavigation_aio.py rename to src/tof_assistedNavigation_aio.py index 5c96731..a23c523 100644 --- a/src/assistedNavigation_aio.py +++ b/src/tof_assistedNavigation_aio.py @@ -1,234 +1,234 @@ -""" -Usage: - -cd esp-wheelchair -python3 src/assistedNavigation_aio.py - -""" -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * -from drive_mode import * - -# Parameters -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera - -USE_EMERGENCYSTOP = 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 - -THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" -RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" -ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' -LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" -RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -# variable initialization -if(USE_VISUAL_POINT_CLOUD): - import pcl.pcl_visualization - viewer_front = pcl.pcl_visualization.PCLVisualizering() - viewer_back = pcl.pcl_visualization.PCLVisualizering() -inputLinear = None -inputAngular = None -manualMode = ManualMode() -repelentMode = RepelentMode() -userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -mode = DriveMode(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 applyYRotation(point_Cloud, theta): - """Apply a Y axis rotation matrix to pointcloud """ - rot = [ - [ math.cos(theta), 0, math.sin(theta)], - [ 0 , 1, 0 ], - [-math.sin(theta), 0, math.cos(theta)] - ] - height = point_Cloud.shape[0] - width = point_Cloud.shape[1] - rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) - for i in range(len(point_Cloud)): - rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) - return rotated_point_Cloud - -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 modeCallBack(msg): - """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" - """ - to change the mode do - rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Mode to Manual") - mode.setMode(manualMode) - elif(msg.data == 2): - print("Changing Mode to Repelent") - mode.setMode(repelentMode) - -def repelentFieldCallBack(msg): - """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ - """ - to change the function do - rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Repelelent field to Linear") - repelentMode.setFunction(msg.data) - elif(msg.data == 2): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - elif(msg.data == 3): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - -def pointCloudCallback(msg, args): - """ Callback function for front ToF sensor """ - # parse arg consist of boolean front and int angle - - front = args[0] - angle = args[1] - - # change from pointcloud2 to numpy - pc = ros_numpy.numpify(msg) - Pointcloud_array = pointCloudToNumpyArray(pc) - - # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix - # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine - # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - rotated_Pointcloud_array = Pointcloud_array - - # visualize if USE_VISUAL_POINT_CLOUD is TRUE - if(USE_VISUAL_POINT_CLOUD): - if(front): - visualizePointCloud(viewer_front, rotated_Pointcloud_array) - else: - visualizePointCloud(viewer_back, rotated_Pointcloud_array) - # find the nearest point and store it at repelent Class - minDist = np.nanmin(rotated_Pointcloud_array[:,2]) - if(front): - repelentMode.setDistanceFront(minDist) - else: - repelentMode.setDistanceBack(minDist) - -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 - inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = mode.control(inputLinear,inputAngular) - - # if the minimum distance is within a certaun threshold then brake - if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF - print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # 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 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) - - if (USE_SIMULATION): - # publish the TWIST output - twist = Twist() - twist.linear.x = x - twist.angular.z = -1*z - assisted_navigation_pub.publish(twist) - - # publish the PWM output - r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - print("left : ", l, ", right : ",r) - - pub_l.publish(l) - pub_r.publish(r) - -if __name__ == "__main__": - # init main loop - rospy.init_node('assisted_Navigation') - - # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode - mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) - - mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) - - # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) - pub_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 pointlcloud subscriber for ToF sensor - # ToF 1 for the back hence first arg is False - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) - # ToF 2 for the front hence first arg is True - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - - print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") - rospy.spin() +""" +Usage: + +cd esp-wheelchair +python3 src/assistedNavigation_aio.py + +""" +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from manual_control import * +from repelent_field_control import * +from user_input_handler import * +from drive_mode import * + +# Parameters +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera + +USE_EMERGENCYSTOP = 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 + +THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 + +LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" +RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' +LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" +RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" + +# variable initialization +if(USE_VISUAL_POINT_CLOUD): + import pcl.pcl_visualization + viewer_front = pcl.pcl_visualization.PCLVisualizering() + viewer_back = pcl.pcl_visualization.PCLVisualizering() +inputLinear = None +inputAngular = None +manualMode = ManualMode() +repelentMode = RepelentMode() +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +mode = DriveMode(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 applyYRotation(point_Cloud, theta): + """Apply a Y axis rotation matrix to pointcloud """ + rot = [ + [ math.cos(theta), 0, math.sin(theta)], + [ 0 , 1, 0 ], + [-math.sin(theta), 0, math.cos(theta)] + ] + height = point_Cloud.shape[0] + width = point_Cloud.shape[1] + rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) + for i in range(len(point_Cloud)): + rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) + return rotated_point_Cloud + +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 modeCallBack(msg): + """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" + """ + to change the mode do + rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Mode to Manual") + mode.setMode(manualMode) + elif(msg.data == 2): + print("Changing Mode to Repelent") + mode.setMode(repelentMode) + +def repelentFieldCallBack(msg): + """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ + """ + to change the function do + rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Repelelent field to Linear") + repelentMode.setFunction(msg.data) + elif(msg.data == 2): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + elif(msg.data == 3): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + +def pointCloudCallback(msg, args): + """ Callback function for front ToF sensor """ + # parse arg consist of boolean front and int angle + + front = args[0] + angle = args[1] + + # change from pointcloud2 to numpy + pc = ros_numpy.numpify(msg) + Pointcloud_array = pointCloudToNumpyArray(pc) + + # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix + # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine + # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) + rotated_Pointcloud_array = Pointcloud_array + + # visualize if USE_VISUAL_POINT_CLOUD is TRUE + if(USE_VISUAL_POINT_CLOUD): + if(front): + visualizePointCloud(viewer_front, rotated_Pointcloud_array) + else: + visualizePointCloud(viewer_back, rotated_Pointcloud_array) + # find the nearest point and store it at repelent Class + minDist = np.nanmin(rotated_Pointcloud_array[:,2]) + if(front): + repelentMode.setDistanceFront(minDist) + else: + repelentMode.setDistanceBack(minDist) + +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 + inputLinear,inputAngular = userInputHandler.getUserInput() + print("inputLinear,inputAngular : ", inputLinear,inputAngular) + outputLinear,outputAngular = mode.control(inputLinear,inputAngular) + + # if the minimum distance is within a certaun threshold then brake + if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # 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 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) + + if (USE_SIMULATION): + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("left : ", l, ", right : ",r) + + pub_l.publish(l) + pub_r.publish(r) + +if __name__ == "__main__": + # init main loop + rospy.init_node('assisted_Navigation') + + # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode + mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) + + mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) + + # initialize wheels publisher + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_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 pointlcloud subscriber for ToF sensor + # ToF 1 for the back hence first arg is False + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) + # ToF 2 for the front hence first arg is True + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() \ No newline at end of file diff --git a/src/drive_controller.py b/src/tof_drive_controller.py similarity index 97% rename from src/drive_controller.py rename to src/tof_drive_controller.py index b060596..463c626 100644 --- a/src/drive_controller.py +++ b/src/tof_drive_controller.py @@ -1,165 +1,165 @@ -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * -from drive_mode import * - -# Parameters -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera - -USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP -USE_SIMULATION = True - -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 - -THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -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" - -ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' - -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -inputLinear = None -inputAngular = None -manualMode = ManualMode() -repelentMode = RepelentMode() -userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -mode = DriveMode(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 modeCallBack(msg): - """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" - """ - to change the mode do - rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Mode to Manual") - mode.setMode(manualMode) - elif(msg.data == 2): - print("Changing Mode to Repelent") - mode.setMode(repelentMode) - -def repelentFieldCallBack(msg): - """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ - """ - to change the function do - rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Repelelent field to Linear") - repelentMode.setFunction(msg.data) - elif(msg.data == 2): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - elif(msg.data == 3): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - -def minDistanceCallback(msg, front): - """ Callback function for front ToF sensor """ - # subscribe to MinDist topic to feed into repelent Mode - print("ToF distance : ", msg.data) - if(front): - repelentMode.setDistanceFront(msg.data) - else: - repelentMode.setDistanceBack(msg.data) - -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 - inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = mode.control(inputLinear,inputAngular) - - # if the minimum distance is within a certaun threshold then brake - if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF - print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF - 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) - - if (USE_SIMULATION): - # publish the TWIST output - twist = Twist() - twist.linear.x = x - twist.angular.z = -1*z - assisted_navigation_pub.publish(twist) - - # publish the PWM output - r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - print("left : ", l, ", right : ",r) - - pub_l.publish(l) - pub_r.publish(r) - -if __name__ == "__main__": - # init main loop - rospy.init_node('AssistedNavigation_main') - - # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode - mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) - - mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) - - # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) - pub_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) - - min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Float64, minDistanceCallback, True) - min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Float64, minDistanceCallback, False) - - print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") - rospy.spin() +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from manual_control import * +from repelent_field_control import * +from user_input_handler import * +from drive_mode import * + +# Parameters +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera + +USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_SIMULATION = True + +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 + +THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold + +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 + +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" + +ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' + +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" + +inputLinear = None +inputAngular = None +manualMode = ManualMode() +repelentMode = RepelentMode() +userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) +mode = DriveMode(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 modeCallBack(msg): + """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" + """ + to change the mode do + rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Mode to Manual") + mode.setMode(manualMode) + elif(msg.data == 2): + print("Changing Mode to Repelent") + mode.setMode(repelentMode) + +def repelentFieldCallBack(msg): + """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ + """ + to change the function do + rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 + """ + if(msg.data == 1): + print("Changing Repelelent field to Linear") + repelentMode.setFunction(msg.data) + elif(msg.data == 2): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + elif(msg.data == 3): + print("Changing Repelelent field to Quadratic") + repelentMode.setFunction(msg.data) + +def minDistanceCallback(msg, front): + """ Callback function for front ToF sensor """ + # subscribe to MinDist topic to feed into repelent Mode + print("ToF distance : ", msg.data) + if(front): + repelentMode.setDistanceFront(msg.data) + else: + repelentMode.setDistanceBack(msg.data) + +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 + inputLinear,inputAngular = userInputHandler.getUserInput() + print("inputLinear,inputAngular : ", inputLinear,inputAngular) + outputLinear,outputAngular = mode.control(inputLinear,inputAngular) + + # if the minimum distance is within a certaun threshold then brake + if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF + 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) + + if (USE_SIMULATION): + # publish the TWIST output + twist = Twist() + twist.linear.x = x + twist.angular.z = -1*z + assisted_navigation_pub.publish(twist) + + # publish the PWM output + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + print("left : ", l, ", right : ",r) + + pub_l.publish(l) + pub_r.publish(r) + +if __name__ == "__main__": + # init main loop + rospy.init_node('AssistedNavigation_main') + + # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode + mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) + + mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) + + # initialize wheels publisher + pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + pub_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) + + min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Float64, minDistanceCallback, True) + min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Float64, minDistanceCallback, False) + + print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") + rospy.spin() \ No newline at end of file diff --git a/src/tof_handler.py b/src/tof_handler.py index 21119f0..0d6eb2e 100644 --- a/src/tof_handler.py +++ b/src/tof_handler.py @@ -1,113 +1,113 @@ - -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * - -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -# variable initialization -if(USE_VISUAL_POINT_CLOUD): - import pcl.pcl_visualization - viewer_front = pcl.pcl_visualization.PCLVisualizering() - viewer_back = pcl.pcl_visualization.PCLVisualizering() - - -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 applyYRotation(point_Cloud, theta): - """Apply a Y axis rotation matrix to pointcloud """ - rot = [ - [ math.cos(theta), 0, math.sin(theta)], - [ 0 , 1, 0 ], - [-math.sin(theta), 0, math.cos(theta)] - ] - height = point_Cloud.shape[0] - width = point_Cloud.shape[1] - rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) - for i in range(len(point_Cloud)): - rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) - return rotated_point_Cloud - -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 pointCloudCallback(msg, args): - """ Callback function for front ToF sensor """ - # parse arg consist of boolean front and int angle - - front = args[0] - angle = args[1] - - # change from pointcloud2 to numpy - pc = ros_numpy.numpify(msg) - Pointcloud_array = pointCloudToNumpyArray(pc) - - # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix - # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine - # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - - # visualize if USE_VISUAL_POINT_CLOUD is TRUE - if(USE_VISUAL_POINT_CLOUD): - if(front): - visualizePointCloud(viewer_front, Pointcloud_array) - else: - visualizePointCloud(viewer_back, Pointcloud_array) - - print(Pointcloud_array.shape) - # find the nearest point and store it at repelent Class - minDist = np.nanmin(Pointcloud_array[:,2]) - print(minDist) - # publish the minimum distance to the MIN_DIST TOPIC - if(front): - min_dist_front_pub.publish(minDist) - print("Minimum distance front : ", minDist) - else: - min_dist_back_pub.publish(minDist) - print("Minimum distance back : ", minDist) - -if __name__ == "__main__": - # init main loop - rospy.init_node('AssistedNavigation_tof') - - # initialize pointlcloud subscriber for ToF sensor - # ToF 1 for the back hence first arg is False - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) - # ToF 2 for the front hence first arg is True - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - - min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Float64, queue_size=3) - min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Float64, queue_size=3) - - print("publishing to " + MIN_DIST_FRONT_TOPIC + " and " + MIN_DIST_BACK_TOPIC + " Spinning...") - rospy.spin() + +import rospy +from std_msgs.msg import Float64, Int16 +from sensor_msgs.msg import PointCloud2, Image +from geometry_msgs.msg import Twist +import ros_numpy + +import pcl +import numpy as np +import math +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +from manual_control import * +from repelent_field_control import * +from user_input_handler import * + +USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output +MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" +MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" + +TOF_1_PITCH = 11 +TOF_2_PITCH = -5 + +# variable initialization +if(USE_VISUAL_POINT_CLOUD): + import pcl.pcl_visualization + viewer_front = pcl.pcl_visualization.PCLVisualizering() + viewer_back = pcl.pcl_visualization.PCLVisualizering() + + +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 applyYRotation(point_Cloud, theta): + """Apply a Y axis rotation matrix to pointcloud """ + rot = [ + [ math.cos(theta), 0, math.sin(theta)], + [ 0 , 1, 0 ], + [-math.sin(theta), 0, math.cos(theta)] + ] + height = point_Cloud.shape[0] + width = point_Cloud.shape[1] + rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) + for i in range(len(point_Cloud)): + rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) + return rotated_point_Cloud + +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 pointCloudCallback(msg, args): + """ Callback function for front ToF sensor """ + # parse arg consist of boolean front and int angle + + front = args[0] + angle = args[1] + + # change from pointcloud2 to numpy + pc = ros_numpy.numpify(msg) + Pointcloud_array = pointCloudToNumpyArray(pc) + + # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix + # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine + # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) + + # visualize if USE_VISUAL_POINT_CLOUD is TRUE + if(USE_VISUAL_POINT_CLOUD): + if(front): + visualizePointCloud(viewer_front, Pointcloud_array) + else: + visualizePointCloud(viewer_back, Pointcloud_array) + + print(Pointcloud_array.shape) + # find the nearest point and store it at repelent Class + minDist = np.nanmin(Pointcloud_array[:,2]) + print(minDist) + # publish the minimum distance to the MIN_DIST TOPIC + if(front): + min_dist_front_pub.publish(minDist) + print("Minimum distance front : ", minDist) + else: + min_dist_back_pub.publish(minDist) + print("Minimum distance back : ", minDist) + +if __name__ == "__main__": + # init main loop + rospy.init_node("Tof_assistedNavigation") + + # initialize pointlcloud subscriber for ToF sensor + # ToF 1 for the back hence first arg is False + point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) + # ToF 2 for the front hence first arg is True + point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) + + min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Float64, queue_size=3) + min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Float64, queue_size=3) + + print("publishing to " + MIN_DIST_FRONT_TOPIC + " and " + MIN_DIST_BACK_TOPIC + " Spinning...") + rospy.spin() \ No newline at end of file diff --git a/src/twist_to_pwm.py b/src/twist_to_pwm.py index 70ad2c6..4ed9f9c 100644 --- a/src/twist_to_pwm.py +++ b/src/twist_to_pwm.py @@ -1,62 +1,62 @@ -# 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): - if True: - 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)) - else: - rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") - - - -sub = rospy.Subscriber("/cmd_vel", Twist, cb) - -rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") -rospy.spin() +# 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): + if True: + 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)) + else: + rospy.logwarn_throttle(1, "Emergency stop active. Ignoring cmd_vel") + + + +sub = rospy.Subscriber("/cmd_vel", Twist, cb) + +rospy.loginfo("Subscribed to /cmd_vel, will publish wheelchair PWM. Spinning...") +rospy.spin() From 398ef708ecf5fc0562232a3725997d179b78f146 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 5 Apr 2023 16:33:58 +0200 Subject: [PATCH 63/83] fix a mistake with output linear --- src/ir_drive_controller.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py index b531cfb..44939aa 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive_controller.py @@ -59,7 +59,7 @@ def userInputCallback(msg, right): # call the current Mode control function to get the adjusted output inputLinear,inputAngular = userInputHandler.getUserInput() print("inputLinear,inputAngular : ", inputLinear,inputAngular) - + outputLinear, outputAngular = inputLinear, inputAngular # if the minimum distance is within a certaun threshold then brake if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") @@ -68,7 +68,6 @@ def userInputCallback(msg, right): print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 - outputLinear, outputAngular = inputLinear,inputAngular # publish the output to wheels rospy.loginfo_throttle(5, "Publishing pwm..") x = max(min(outputLinear, 1.0), -1.0) From a5ee64f767c0f2fde6ea91adb51bec3074e029f9 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 5 Apr 2023 17:51:43 +0200 Subject: [PATCH 64/83] ir publisher overhaul --- src/ir_handler.py | 61 +++++++++++++++++++++++++---------------------- 1 file changed, 33 insertions(+), 28 deletions(-) diff --git a/src/ir_handler.py b/src/ir_handler.py index ca9203d..766ba6d 100644 --- a/src/ir_handler.py +++ b/src/ir_handler.py @@ -5,11 +5,12 @@ import Jetson.GPIO as GPIO +print("Initializing IR Publisher") # Set up GPIO pins -pin_front_right = 1 -pin_front_left = 2 -pin_back_right = 3 -pin_back_left = 4 +pin_front_right = 7 +pin_front_left = 11 +pin_back_right = 13 +pin_back_left = 15 GPIO.setmode(GPIO.BOARD) GPIO.setup(pin_front_right, GPIO.IN) @@ -18,9 +19,10 @@ GPIO.setup(pin_back_left, GPIO.IN) # Set up ROS node -rospy.init_node("Ir_handler") +rospy.init_node("Ir_publisher") +print("Initializing IR_publisher initialized") -# Set up publishers for each distance topic +# Set up publishers for each IR sensor DIST_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_right" DIST_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_left" DIST_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_right" @@ -31,25 +33,28 @@ pub_back_right = rospy.Publisher(DIST_BACK_RIGHT_TOPIC, Int16, queue_size=10) pub_back_left = rospy.Publisher(DIST_BACK_LEFT_TOPIC, Int16, queue_size=10) -# Define callback function for GPIO pin changes -def gpio_callback(channel): - if channel == pin_front_right: - pub_front_right.publish(GPIO.input(channel)) - elif channel == pin_front_left: - pub_front_left.publish(GPIO.input(channel)) - elif channel == pin_back_right: - pub_back_right.publish(GPIO.input(channel)) - elif channel == pin_back_left: - pub_back_left.publish(GPIO.input(channel)) - -# Set up GPIO pin event detection -GPIO.add_event_detect(pin_front_right, GPIO.BOTH, callback=gpio_callback) -GPIO.add_event_detect(pin_front_left, GPIO.BOTH, callback=gpio_callback) -GPIO.add_event_detect(pin_back_right, GPIO.BOTH, callback=gpio_callback) -GPIO.add_event_detect(pin_back_left, GPIO.BOTH, callback=gpio_callback) - -# Spin ROS node -rospy.spin() - -# Clean up GPIO pins -GPIO.cleanup() +def main(): + while True: + # read value from sensor and publish publish them to their topic + # front right + value_front_right = GPIO.input(pin_front_right) + print("front right : " + str(value_front_right)) + pub_front_right.publish(value_front_right) + + # front left + value_front_left = GPIO.input(pin_front_left) + print("front left : " + str(value_front_left)) + pub_front_left.publish(value_front_left) + + # back right + value_back_right = GPIO.input(pin_back_right) + print("back right : " + str(value_back_right)) + pub_back_right.publish(value_back_right) + + # back left + value_back_left = GPIO.input(pin_back_left) + print("back left : " + str(value_back_left)) + pub_back_left.publish(value_back_left) + + +main() \ No newline at end of file From 1f7ddbf51726fb949ed969c47e527f8b8a7f7f44 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 6 Apr 2023 14:55:34 +0200 Subject: [PATCH 65/83] refactor IRversion of assisted navigation --- src/ir_drive_controller.py | 50 ++++++++++++++++---------------------- src/ir_handler.py | 18 +++++++------- 2 files changed, 30 insertions(+), 38 deletions(-) diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py index 44939aa..696450a 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive_controller.py @@ -1,17 +1,12 @@ import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image +from std_msgs.msg import Int16 from geometry_msgs.msg import Twist -import ros_numpy -import pcl import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError from user_input_handler import UserInputHandler from IR_State import IRState + # Parameters USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP USE_SIMULATION = True @@ -21,24 +16,21 @@ OUTPUT_PWM_MIN = 0 # Output PWM minimum value OUTPUT_PWM_RANGE = 20 # Output PWM range value -THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold - 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 -DIST_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_right" -DIST_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_left" -DIST_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_right" -DIST_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_left" +IR_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_right" +IR_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_left" +IR_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_right" +IR_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_left" -inputLinear = None -inputAngular = None userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) irState = IRState() @@ -54,17 +46,17 @@ def userInputCallback(msg, right): 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) + userInputHandler.setUserInput(msg, right) # call the current Mode control function to get the adjusted output inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear,inputAngular : ", inputLinear,inputAngular) + print("inputLinear, inputAngular : ", inputLinear,inputAngular) outputLinear, outputAngular = inputLinear, inputAngular # if the minimum distance is within a certaun threshold then brake - if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF + if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and inputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF + elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and inputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 @@ -74,20 +66,20 @@ def userInputCallback(msg, right): 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): - # publish the TWIST output twist = Twist() twist.linear.x = x twist.angular.z = -1*z assisted_navigation_pub.publish(twist) - # publish the PWM output + # publish the PWM output to the motor r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) print("left : ", l, ", right : ",r) - pub_l.publish(l) - pub_r.publish(r) + pub_motor_l.publish(l) + pub_motor_r.publish(r) def irSensorCallback(msg, pos): irState.set(pos,msg.data) @@ -98,8 +90,8 @@ def irSensorCallback(msg, pos): rospy.init_node('Ir_AssistedNavigation_main') # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) - pub_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) + 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 @@ -110,10 +102,10 @@ def irSensorCallback(msg, pos): user_input_sub_l = rospy.Subscriber(LEFT_MOTOR_TOPIC_INPUT, Int16, userInputCallback, False) #Initialize subscriber for Ir sensor topic - front_right_sub = rospy.Subscriber(DIST_FRONT_RIGHT_TOPIC, Int16, irSensorCallback, 1) - front_left_sub = rospy.Subscriber(DIST_FRONT_LEFT_TOPIC, Int16, irSensorCallback, 2) - back_right_sub = rospy.Subscriber(DIST_BACK_RIGHT_TOPIC, Int16, irSensorCallback, 3) - back_left_sub = rospy.Subscriber(DIST_BACK_LEFT_TOPIC, Int16, irSensorCallback, 4) + front_right_sub = rospy.Subscriber(IR_FRONT_RIGHT_TOPIC, Int16, irSensorCallback, 1) + front_left_sub = rospy.Subscriber(IR_FRONT_LEFT_TOPIC, Int16, irSensorCallback, 2) + back_right_sub = rospy.Subscriber(IR_BACK_RIGHT_TOPIC, Int16, irSensorCallback, 3) + back_left_sub = rospy.Subscriber(IR_BACK_LEFT_TOPIC, Int16, irSensorCallback, 4) print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") rospy.spin() diff --git a/src/ir_handler.py b/src/ir_handler.py index 766ba6d..304dce1 100644 --- a/src/ir_handler.py +++ b/src/ir_handler.py @@ -23,15 +23,15 @@ print("Initializing IR_publisher initialized") # Set up publishers for each IR sensor -DIST_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_right" -DIST_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front_left" -DIST_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_right" -DIST_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back_left" - -pub_front_right = rospy.Publisher(DIST_FRONT_RIGHT_TOPIC, Int16, queue_size=10) -pub_front_left = rospy.Publisher(DIST_FRONT_LEFT_TOPIC, Int16, queue_size=10) -pub_back_right = rospy.Publisher(DIST_BACK_RIGHT_TOPIC, Int16, queue_size=10) -pub_back_left = rospy.Publisher(DIST_BACK_LEFT_TOPIC, Int16, queue_size=10) +IR_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_right" +IR_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_left" +IR_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_right" +IR_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_left" + +pub_front_right = rospy.Publisher(IR_FRONT_RIGHT_TOPIC, Int16, queue_size=10) +pub_front_left = rospy.Publisher(IR_FRONT_LEFT_TOPIC, Int16, queue_size=10) +pub_back_right = rospy.Publisher(IR_BACK_RIGHT_TOPIC, Int16, queue_size=10) +pub_back_left = rospy.Publisher(IR_BACK_LEFT_TOPIC, Int16, queue_size=10) def main(): while True: From d9af7dd0d324c8e49147fff419a4ac638f1c268a Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 6 Apr 2023 14:58:26 +0200 Subject: [PATCH 66/83] change file name ro be more consistent --- src/ir_drive_controller.py | 2 +- src/ir_state.py | 37 +++++++++++++++++++++++++++++++++++++ 2 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 src/ir_state.py diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py index 696450a..ace9c27 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive_controller.py @@ -5,7 +5,7 @@ import numpy as np from user_input_handler import UserInputHandler -from IR_State import IRState +from ir_state import IRState # Parameters USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP diff --git a/src/ir_state.py b/src/ir_state.py new file mode 100644 index 0000000..951faa7 --- /dev/null +++ b/src/ir_state.py @@ -0,0 +1,37 @@ +class IRState: + """ Manual Mode no modification between user input and output """ + _FRONT_RIGHT = 1 + _FRONT_LEFT = 2 + _BACK_RIGHT = 3 + _BACK_LEFT = 4 + def __init__(self): + self.dist_front_right = 0 + self.dist_front_left = 0 + self.dist_back_right = 0 + self.dist_back_left = 0 + return + + def set(self, input, pos): + if pos == IRState._FRONT_RIGHT: + self.dist_front_right = input + elif pos == IRState._FRONT_LEFT: + self.dist_front_left = input + elif pos == IRState._BACK_RIGHT: + self.dist_back_right = input + elif pos == IRState._BACK_LEFT: + self.dist_back_left = input + + def get(self,pos): + if pos == IRState._FRONT_RIGHT: + return self.dist_front_right + elif pos == IRState._FRONT_LEFT: + return self.dist_front_left + elif pos == IRState._BACK_RIGHT: + return self.dist_back_right + elif pos == IRState._BACK_LEFT: + return self.dist_back_left + + + + def control(self, inputLinear, inputAngular): + return inputLinear,inputAngular \ No newline at end of file From 7e23a16891c94941240f89b0213146ec67e1a48b Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 6 Apr 2023 15:40:03 +0200 Subject: [PATCH 67/83] delete previous impelmentation using ToF and change the IR publisher to using an array --- src/IR_State.py | 39 ++--- src/drive_mode.py | 12 -- src/ir_drive_controller.py | 36 +++-- src/ir_handler.py | 60 -------- src/ir_sensor_publisher.py | 35 +++++ src/ir_state.py | 39 ++--- src/manual_control.py | 7 - src/repelent_field_control.py | 71 --------- src/tof_assistedNavigation_aio.py | 234 ------------------------------ src/tof_drive_controller.py | 165 --------------------- src/tof_handler.py | 113 --------------- src/twist_to_pwm.py | 23 ++- 12 files changed, 85 insertions(+), 749 deletions(-) delete mode 100644 src/drive_mode.py delete mode 100644 src/ir_handler.py create mode 100644 src/ir_sensor_publisher.py delete mode 100644 src/manual_control.py delete mode 100644 src/repelent_field_control.py delete mode 100644 src/tof_assistedNavigation_aio.py delete mode 100644 src/tof_drive_controller.py delete mode 100644 src/tof_handler.py diff --git a/src/IR_State.py b/src/IR_State.py index ca63630..fc88b38 100644 --- a/src/IR_State.py +++ b/src/IR_State.py @@ -1,37 +1,16 @@ class IRState: """ Manual Mode no modification between user input and output """ - _FRONT_RIGHT = 1 - _FRONT_LEFT = 2 - _BACK_RIGHT = 3 - _BACK_LEFT = 4 + _FRONT_RIGHT_ID = 1 + _FRONT_LEFT_ID = 2 + _BACK_RIGHT_ID = 3 + _BACK_LEFT_ID = 4 def __init__(self): - self.dist_front_right = 0 - self.dist_front_left = 0 - self.dist_back_right = 0 - self.dist_back_left = 0 + self.ir_sensor = [0,0,0,0] return - def set(self, input, pos): - if pos == IRState._FRONT_RIGHT: - self.dist_front_right = input - elif pos == IRState._FRONT_LEFT: - self.dist_front_left = input - elif pos == IRState._BACK_RIGHT: - self.dist_back_right = input - elif pos == IRState._BACK_LEFT: - self.dist_back_left = input + def set(self, input): + self.ir_sensor = input + return def get(self,pos): - if pos == IRState._FRONT_RIGHT: - return self.dist_front_right - elif pos == IRState._FRONT_LEFT: - return self.dist_front_left - elif pos == IRState._BACK_RIGHT: - return self.dist_back_right - elif pos == IRState._BACK_LEFT: - return self.dist_back_left - - - - def control(self, inputLinear, inputAngular): - return inputLinear,inputAngular \ No newline at end of file + return self.ir_sensor[pos] \ No newline at end of file diff --git a/src/drive_mode.py b/src/drive_mode.py deleted file mode 100644 index 1831ff6..0000000 --- a/src/drive_mode.py +++ /dev/null @@ -1,12 +0,0 @@ -class DriveMode: - """ this class is a place holder for the current active class """ - def __init__(self,mode): - self.Mode = mode - return - - def setMode(self, mode): - self.Mode = mode - return - - def control(self, inputLinear, inputAngular): - return self.Mode.control(inputLinear,inputAngular) \ No newline at end of file diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py index ace9c27..57d28cb 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive_controller.py @@ -1,11 +1,23 @@ +""" +Usage: + +python ir_drive_controller.py +python ir_publisher.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 +from std_msgs.msg import Int16, Int16MultiArray from geometry_msgs.msg import Twist import numpy as np from user_input_handler import UserInputHandler -from ir_state import IRState +from IR_State import IRState # Parameters USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP @@ -26,10 +38,11 @@ ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' # Set up publishers for each distance topic -IR_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_right" -IR_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_left" -IR_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_right" -IR_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_left" +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() @@ -81,8 +94,8 @@ def userInputCallback(msg, right): pub_motor_l.publish(l) pub_motor_r.publish(r) -def irSensorCallback(msg, pos): - irState.set(pos,msg.data) +def irSensorCallback(msg): + irState.set(msg.data) return if __name__ == "__main__": @@ -101,11 +114,8 @@ def irSensorCallback(msg, pos): 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 topic - front_right_sub = rospy.Subscriber(IR_FRONT_RIGHT_TOPIC, Int16, irSensorCallback, 1) - front_left_sub = rospy.Subscriber(IR_FRONT_LEFT_TOPIC, Int16, irSensorCallback, 2) - back_right_sub = rospy.Subscriber(IR_BACK_RIGHT_TOPIC, Int16, irSensorCallback, 3) - back_left_sub = rospy.Subscriber(IR_BACK_LEFT_TOPIC, Int16, irSensorCallback, 4) + #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() diff --git a/src/ir_handler.py b/src/ir_handler.py deleted file mode 100644 index 304dce1..0000000 --- a/src/ir_handler.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python - -import rospy -from std_msgs.msg import Int16 - -import Jetson.GPIO as GPIO - -print("Initializing IR Publisher") -# Set up GPIO pins -pin_front_right = 7 -pin_front_left = 11 -pin_back_right = 13 -pin_back_left = 15 - -GPIO.setmode(GPIO.BOARD) -GPIO.setup(pin_front_right, GPIO.IN) -GPIO.setup(pin_front_left, GPIO.IN) -GPIO.setup(pin_back_right, GPIO.IN) -GPIO.setup(pin_back_left, GPIO.IN) - -# Set up ROS node -rospy.init_node("Ir_publisher") -print("Initializing IR_publisher initialized") - -# Set up publishers for each IR sensor -IR_FRONT_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_right" -IR_FRONT_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/front_left" -IR_BACK_RIGHT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_right" -IR_BACK_LEFT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/ir/back_left" - -pub_front_right = rospy.Publisher(IR_FRONT_RIGHT_TOPIC, Int16, queue_size=10) -pub_front_left = rospy.Publisher(IR_FRONT_LEFT_TOPIC, Int16, queue_size=10) -pub_back_right = rospy.Publisher(IR_BACK_RIGHT_TOPIC, Int16, queue_size=10) -pub_back_left = rospy.Publisher(IR_BACK_LEFT_TOPIC, Int16, queue_size=10) - -def main(): - while True: - # read value from sensor and publish publish them to their topic - # front right - value_front_right = GPIO.input(pin_front_right) - print("front right : " + str(value_front_right)) - pub_front_right.publish(value_front_right) - - # front left - value_front_left = GPIO.input(pin_front_left) - print("front left : " + str(value_front_left)) - pub_front_left.publish(value_front_left) - - # back right - value_back_right = GPIO.input(pin_back_right) - print("back right : " + str(value_back_right)) - pub_back_right.publish(value_back_right) - - # back left - value_back_left = GPIO.input(pin_back_left) - print("back left : " + str(value_back_left)) - pub_back_left.publish(value_back_left) - - -main() \ No newline at end of file diff --git a/src/ir_sensor_publisher.py b/src/ir_sensor_publisher.py new file mode 100644 index 0000000..412fb86 --- /dev/null +++ b/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_state.py b/src/ir_state.py index 951faa7..d3c5d0c 100644 --- a/src/ir_state.py +++ b/src/ir_state.py @@ -1,37 +1,16 @@ class IRState: """ Manual Mode no modification between user input and output """ - _FRONT_RIGHT = 1 - _FRONT_LEFT = 2 - _BACK_RIGHT = 3 - _BACK_LEFT = 4 + _FRONT_RIGHT_ID = 1 + _FRONT_LEFT_ID = 2 + _BACK_RIGHT_ID = 3 + _BACK_LEFT_ID = 4 def __init__(self): - self.dist_front_right = 0 - self.dist_front_left = 0 - self.dist_back_right = 0 - self.dist_back_left = 0 + self.ir_sensor = [0,0,0,0] return - def set(self, input, pos): - if pos == IRState._FRONT_RIGHT: - self.dist_front_right = input - elif pos == IRState._FRONT_LEFT: - self.dist_front_left = input - elif pos == IRState._BACK_RIGHT: - self.dist_back_right = input - elif pos == IRState._BACK_LEFT: - self.dist_back_left = input + def set(self, input): + self.ir_sensor = input + return def get(self,pos): - if pos == IRState._FRONT_RIGHT: - return self.dist_front_right - elif pos == IRState._FRONT_LEFT: - return self.dist_front_left - elif pos == IRState._BACK_RIGHT: - return self.dist_back_right - elif pos == IRState._BACK_LEFT: - return self.dist_back_left - - - - def control(self, inputLinear, inputAngular): - return inputLinear,inputAngular \ No newline at end of file + return self.ir_sensor[pos] \ No newline at end of file diff --git a/src/manual_control.py b/src/manual_control.py deleted file mode 100644 index 76d88b4..0000000 --- a/src/manual_control.py +++ /dev/null @@ -1,7 +0,0 @@ -class ManualMode: - """ Manual Mode no modification between user input and output """ - def __init__(self): - return - - def control(self, inputLinear, inputAngular): - return inputLinear,inputAngular \ No newline at end of file diff --git a/src/repelent_field_control.py b/src/repelent_field_control.py deleted file mode 100644 index 87e6941..0000000 --- a/src/repelent_field_control.py +++ /dev/null @@ -1,71 +0,0 @@ -class RepelentMode: - """ Repelent field mode get slower as the robot get near an obstacle """ - DistFront = 1 # store nearest distance to front obstacle - DistBack = 1 # store nearest distance to back obstacle - Function = 1 # function defaults to linear - 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 setFunction(self, function): - """ set memeber variable function """ - self.Function = function - print("Changing Repelelent field to ", self.Function) - - def control(self, inputLinear, inputAngular): - """ give an output based of the distance to an obstacle """ - print("Minimum Distance Back : ", self.DistBack) - if(self.Function == 1): # Linear - outputLinear, outputAngular = self.linear(inputLinear,inputAngular) - elif(self.Function == 2): # Quadratic - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - elif(self.Function == 3): # Quadratic - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - else: # if undefined defaults to linear - outputLinear, outputAngular = self.quadratic(inputLinear,inputAngular) - return outputLinear,outputAngular - - def linear(self, inputLinear,inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = self.DistFront - elif inputLinear < 0: - outputLinear = -1*self.DistBack - else: - outputLinear = 0 - return outputLinear, inputAngular - - def quadratic(self, inputLinear, inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = 1.5*pow(self.DistFront,2) - elif inputLinear < 0: - outputLinear = -1.5*pow(self.DistBack,2) - else: - outputLinear = 0 - return outputLinear, inputAngular - - def quadratic2(self, inputLinear, inputAngular): - """ adjust speed linearly to the distance of nearest obstacle """ - if inputLinear > 0: - outputLinear = 2*pow(self.DistFront,2) - elif inputLinear < 0: - outputLinear = -2*pow(self.DistBack,2) - else: - outputLinear = 0 - return outputLinear, inputAngular diff --git a/src/tof_assistedNavigation_aio.py b/src/tof_assistedNavigation_aio.py deleted file mode 100644 index a23c523..0000000 --- a/src/tof_assistedNavigation_aio.py +++ /dev/null @@ -1,234 +0,0 @@ -""" -Usage: - -cd esp-wheelchair -python3 src/assistedNavigation_aio.py - -""" -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * -from drive_mode import * - -# Parameters -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera - -USE_EMERGENCYSTOP = 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 - -THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -LEFT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/left" -RIGHT_MOTOR_TOPIC_OUTPUT = "/roboy/pinky/middleware/espchair/wheels/right" -ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' -LEFT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/left/input" -RIGHT_MOTOR_TOPIC_INPUT = "/roboy/pinky/middleware/espchair/wheels/right/input" -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -# variable initialization -if(USE_VISUAL_POINT_CLOUD): - import pcl.pcl_visualization - viewer_front = pcl.pcl_visualization.PCLVisualizering() - viewer_back = pcl.pcl_visualization.PCLVisualizering() -inputLinear = None -inputAngular = None -manualMode = ManualMode() -repelentMode = RepelentMode() -userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -mode = DriveMode(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 applyYRotation(point_Cloud, theta): - """Apply a Y axis rotation matrix to pointcloud """ - rot = [ - [ math.cos(theta), 0, math.sin(theta)], - [ 0 , 1, 0 ], - [-math.sin(theta), 0, math.cos(theta)] - ] - height = point_Cloud.shape[0] - width = point_Cloud.shape[1] - rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) - for i in range(len(point_Cloud)): - rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) - return rotated_point_Cloud - -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 modeCallBack(msg): - """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" - """ - to change the mode do - rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Mode to Manual") - mode.setMode(manualMode) - elif(msg.data == 2): - print("Changing Mode to Repelent") - mode.setMode(repelentMode) - -def repelentFieldCallBack(msg): - """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ - """ - to change the function do - rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Repelelent field to Linear") - repelentMode.setFunction(msg.data) - elif(msg.data == 2): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - elif(msg.data == 3): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - -def pointCloudCallback(msg, args): - """ Callback function for front ToF sensor """ - # parse arg consist of boolean front and int angle - - front = args[0] - angle = args[1] - - # change from pointcloud2 to numpy - pc = ros_numpy.numpify(msg) - Pointcloud_array = pointCloudToNumpyArray(pc) - - # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix - # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine - # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - rotated_Pointcloud_array = Pointcloud_array - - # visualize if USE_VISUAL_POINT_CLOUD is TRUE - if(USE_VISUAL_POINT_CLOUD): - if(front): - visualizePointCloud(viewer_front, rotated_Pointcloud_array) - else: - visualizePointCloud(viewer_back, rotated_Pointcloud_array) - # find the nearest point and store it at repelent Class - minDist = np.nanmin(rotated_Pointcloud_array[:,2]) - if(front): - repelentMode.setDistanceFront(minDist) - else: - repelentMode.setDistanceBack(minDist) - -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 - inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = mode.control(inputLinear,inputAngular) - - # if the minimum distance is within a certaun threshold then brake - if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF - print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # 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 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) - - if (USE_SIMULATION): - # publish the TWIST output - twist = Twist() - twist.linear.x = x - twist.angular.z = -1*z - assisted_navigation_pub.publish(twist) - - # publish the PWM output - r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - print("left : ", l, ", right : ",r) - - pub_l.publish(l) - pub_r.publish(r) - -if __name__ == "__main__": - # init main loop - rospy.init_node('assisted_Navigation') - - # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode - mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) - - mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) - - # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) - pub_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 pointlcloud subscriber for ToF sensor - # ToF 1 for the back hence first arg is False - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) - # ToF 2 for the front hence first arg is True - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - - print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") - rospy.spin() - \ No newline at end of file diff --git a/src/tof_drive_controller.py b/src/tof_drive_controller.py deleted file mode 100644 index 463c626..0000000 --- a/src/tof_drive_controller.py +++ /dev/null @@ -1,165 +0,0 @@ -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * -from drive_mode import * - -# Parameters -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -USE_REAR_CAMERA = False # USE_REAR_CAMERA if true will publish a data of rear camera - -USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP -USE_SIMULATION = True - -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 - -THRESHOLD_EMERGENCYSTOP = 0.1 # Emergency stop threshold roboy will stop if it detect a point below the thereshold - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -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" - -ASSISTED_NAVIGATION_TOPIC_OUTPUT = '/roboy/pinky/middleware/espchair/wheels/assisted_navigation' - -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -inputLinear = None -inputAngular = None -manualMode = ManualMode() -repelentMode = RepelentMode() -userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -mode = DriveMode(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 modeCallBack(msg): - """ Callback function for topic '/roboy/pinky/middleware/espchair/wheels/mode' to change the drive mode""" - """ - to change the mode do - rostopic pub /roboy/pinky/middleware/espchair/wheels/mode std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Mode to Manual") - mode.setMode(manualMode) - elif(msg.data == 2): - print("Changing Mode to Repelent") - mode.setMode(repelentMode) - -def repelentFieldCallBack(msg): - """ Callback funtion for '/roboy/pinky/middleware/espchair/wheels/repelent_field' to change the tye of repellent field """ - """ - to change the function do - rostopic pub /roboy/pinky/middleware/espchair/wheels/repelent_field std_msgs/Int16 1 - """ - if(msg.data == 1): - print("Changing Repelelent field to Linear") - repelentMode.setFunction(msg.data) - elif(msg.data == 2): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - elif(msg.data == 3): - print("Changing Repelelent field to Quadratic") - repelentMode.setFunction(msg.data) - -def minDistanceCallback(msg, front): - """ Callback function for front ToF sensor """ - # subscribe to MinDist topic to feed into repelent Mode - print("ToF distance : ", msg.data) - if(front): - repelentMode.setDistanceFront(msg.data) - else: - repelentMode.setDistanceBack(msg.data) - -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 - inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear,inputAngular : ", inputLinear,inputAngular) - outputLinear,outputAngular = mode.control(inputLinear,inputAngular) - - # if the minimum distance is within a certaun threshold then brake - if(repelentMode.getDistanceFront() < THRESHOLD_EMERGENCYSTOP and inputLinear > 0 and USE_EMERGENCYSTOP): # this is the front ToF - print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - elif (repelentMode.getDistanceBack() < THRESHOLD_EMERGENCYSTOP and inputLinear < 0 and USE_EMERGENCYSTOP): # this is the back ToF - 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) - - if (USE_SIMULATION): - # publish the TWIST output - twist = Twist() - twist.linear.x = x - twist.angular.z = -1*z - assisted_navigation_pub.publish(twist) - - # publish the PWM output - r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) - print("left : ", l, ", right : ",r) - - pub_l.publish(l) - pub_r.publish(r) - -if __name__ == "__main__": - # init main loop - rospy.init_node('AssistedNavigation_main') - - # initialize mode subscriber, used for changing the mode through the topic /roboy/pinky/middleware/espchair/wheels/mode - mode_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/mode', Int16, modeCallBack) - - mode_repelent_sub = rospy.Subscriber('/roboy/pinky/middleware/espchair/wheels/repelent_field', Int16, repelentFieldCallBack) - - # initialize wheels publisher - pub_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) - pub_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) - - min_dist_front_pub = rospy.Subscriber(MIN_DIST_FRONT_TOPIC, Float64, minDistanceCallback, True) - min_dist_back_pub = rospy.Subscriber(MIN_DIST_BACK_TOPIC, Float64, minDistanceCallback, False) - - print("publishing to /roboy/pinky/middleware/espchair/wheels/assisted_navigation. Spinning...") - rospy.spin() - \ No newline at end of file diff --git a/src/tof_handler.py b/src/tof_handler.py deleted file mode 100644 index 0d6eb2e..0000000 --- a/src/tof_handler.py +++ /dev/null @@ -1,113 +0,0 @@ - -import rospy -from std_msgs.msg import Float64, Int16 -from sensor_msgs.msg import PointCloud2, Image -from geometry_msgs.msg import Twist -import ros_numpy - -import pcl -import numpy as np -import math -import cv2 -from cv_bridge import CvBridge, CvBridgeError - -from manual_control import * -from repelent_field_control import * -from user_input_handler import * - -USE_VISUAL_POINT_CLOUD = False # USE_VISUAL_POINT_CLOUD if true will open a window that show the ToF sensor output -MIN_DIST_FRONT_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/front" -MIN_DIST_BACK_TOPIC = "/roboy/pinky/middleware/espchair/wheels/dist/back" - -TOF_1_PITCH = 11 -TOF_2_PITCH = -5 - -# variable initialization -if(USE_VISUAL_POINT_CLOUD): - import pcl.pcl_visualization - viewer_front = pcl.pcl_visualization.PCLVisualizering() - viewer_back = pcl.pcl_visualization.PCLVisualizering() - - -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 applyYRotation(point_Cloud, theta): - """Apply a Y axis rotation matrix to pointcloud """ - rot = [ - [ math.cos(theta), 0, math.sin(theta)], - [ 0 , 1, 0 ], - [-math.sin(theta), 0, math.cos(theta)] - ] - height = point_Cloud.shape[0] - width = point_Cloud.shape[1] - rotated_point_Cloud = np.zeros((height * width, 3), dtype=np.float32) - for i in range(len(point_Cloud)): - rotated_point_Cloud[i] = np.dot(point_Cloud[i],rot) - return rotated_point_Cloud - -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 pointCloudCallback(msg, args): - """ Callback function for front ToF sensor """ - # parse arg consist of boolean front and int angle - - front = args[0] - angle = args[1] - - # change from pointcloud2 to numpy - pc = ros_numpy.numpify(msg) - Pointcloud_array = pointCloudToNumpyArray(pc) - - # To maximize the FOV of the TOF sensor we apply a slight pitch (-5 deg for short/fat ToF and 16 deg for long ToF ) so to get the correct distance we apply a Y axis rotation matrix - # Encountered some performance issue, Pointcloud_array shape is (38528, 3) seem to use too many resource and lagging the machine - # rotated_Pointcloud_array = applyYRotation(Pointcloud_array,angle) - - # visualize if USE_VISUAL_POINT_CLOUD is TRUE - if(USE_VISUAL_POINT_CLOUD): - if(front): - visualizePointCloud(viewer_front, Pointcloud_array) - else: - visualizePointCloud(viewer_back, Pointcloud_array) - - print(Pointcloud_array.shape) - # find the nearest point and store it at repelent Class - minDist = np.nanmin(Pointcloud_array[:,2]) - print(minDist) - # publish the minimum distance to the MIN_DIST TOPIC - if(front): - min_dist_front_pub.publish(minDist) - print("Minimum distance front : ", minDist) - else: - min_dist_back_pub.publish(minDist) - print("Minimum distance back : ", minDist) - -if __name__ == "__main__": - # init main loop - rospy.init_node("Tof_assistedNavigation") - - # initialize pointlcloud subscriber for ToF sensor - # ToF 1 for the back hence first arg is False - point_cloud_1_sub = rospy.Subscriber('/tof1_driver/point_cloud', PointCloud2, pointCloudCallback, (False, TOF_1_PITCH)) - # ToF 2 for the front hence first arg is True - point_cloud_2_sub = rospy.Subscriber('/tof2_driver/point_cloud', PointCloud2, pointCloudCallback, (True, TOF_2_PITCH)) - - min_dist_front_pub = rospy.Publisher(MIN_DIST_FRONT_TOPIC, Float64, queue_size=3) - min_dist_back_pub = rospy.Publisher(MIN_DIST_BACK_TOPIC, Float64, queue_size=3) - - print("publishing to " + MIN_DIST_FRONT_TOPIC + " and " + MIN_DIST_BACK_TOPIC + " Spinning...") - rospy.spin() - \ No newline at end of file diff --git a/src/twist_to_pwm.py b/src/twist_to_pwm.py index 4ed9f9c..9f7bba6 100644 --- a/src/twist_to_pwm.py +++ b/src/twist_to_pwm.py @@ -38,23 +38,18 @@ def mapPwm(x, out_min, out_max): def cb(msg): - if True: - 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) + 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)) - 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) + pub_l.publish(int(sign(l)*lPwm)) + pub_r.publish(int(sign(r)*rPwm)) sub = rospy.Subscriber("/cmd_vel", Twist, cb) From 6f17f5aa81927536b58b0fc4a68155f9dddb53bf Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Thu, 6 Apr 2023 15:58:37 +0200 Subject: [PATCH 68/83] add a stand alone python script all in one for drive controller --- src/ir_drive_controller.py | 16 ++- src/ir_drive_controller_aio.py | 183 +++++++++++++++++++++++++++++++++ 2 files changed, 190 insertions(+), 9 deletions(-) create mode 100644 src/ir_drive_controller_aio.py diff --git a/src/ir_drive_controller.py b/src/ir_drive_controller.py index 57d28cb..aa4728a 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive_controller.py @@ -1,8 +1,8 @@ """ Usage: +python ir_sensor_publisher.py python ir_drive_controller.py -python ir_publisher.py and if you are using teleop keyboard use @@ -14,8 +14,6 @@ from std_msgs.msg import Int16, Int16MultiArray from geometry_msgs.msg import Twist -import numpy as np - from user_input_handler import UserInputHandler from IR_State import IRState @@ -62,14 +60,14 @@ def userInputCallback(msg, right): userInputHandler.setUserInput(msg, right) # call the current Mode control function to get the adjusted output - inputLinear,inputAngular = userInputHandler.getUserInput() - print("inputLinear, inputAngular : ", inputLinear,inputAngular) - outputLinear, outputAngular = inputLinear, inputAngular + outputLinear, outputAngular = userInputHandler.getUserInput() + print("inputLinear, inputAngular : ", outputLinear, outputAngular) + # if the minimum distance is within a certaun threshold then brake - if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and inputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and inputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 @@ -100,7 +98,7 @@ def irSensorCallback(msg): if __name__ == "__main__": # init main loop - rospy.init_node('Ir_AssistedNavigation_main') + rospy.init_node('Ir_drive_controller') # initialize wheels publisher pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=1) diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py new file mode 100644 index 0000000..577d7ff --- /dev/null +++ b/src/ir_drive_controller_aio.py @@ -0,0 +1,183 @@ +""" +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 +class UserInputHandler: + """ Manual Mode no modification between user input and output """ + 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, rigth): + """ Set the user input, store them at class member and define IsDefine as true """ + if (rigth): + 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, -30, 30, -1, 1) + inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -30, 30, -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) +# from IR_State import IRState +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] + +# Parameters +USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_SIMULATION = True + +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() + print("inputLinear, inputAngular : ", outputLinear, outputAngular) + + # if the minimum distance is within a certaun threshold then brake + if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + outputLinear = 0 + elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # 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 )) + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + 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 From 904dfd296ed8656abcb359010283a46fcc176217 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 7 Apr 2023 11:38:03 +0200 Subject: [PATCH 69/83] remove unnesesarry code and modify according to IRsensor publisher --- src/ir_drive_controller_aio.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py index 577d7ff..10b8817 100644 --- a/src/ir_drive_controller_aio.py +++ b/src/ir_drive_controller_aio.py @@ -62,6 +62,7 @@ def translate(self, value, leftMin, leftMax, rightMin, rightMax): # Convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightRange) + # from IR_State import IRState class IRState: """ Manual Mode no modification between user input and output """ @@ -100,6 +101,7 @@ def get(self,pos): # 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 @@ -108,12 +110,6 @@ def get(self,pos): 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 @@ -127,10 +123,10 @@ def userInputCallback(msg, right): print("inputLinear, inputAngular : ", outputLinear, outputAngular) # if the minimum distance is within a certaun threshold then brake - if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + if((not irState.get(IRState._FRONT_RIGHT) or not irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + elif ((not irState.get(IRState._BACK_RIGHT) or not irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 From b6aa824555daf9e1ecfb93197efd6386e7e0c3f9 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 7 Apr 2023 11:49:29 +0200 Subject: [PATCH 70/83] fix minor bug --- src/ir_drive_controller_aio.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py index 577d7ff..294fa55 100644 --- a/src/ir_drive_controller_aio.py +++ b/src/ir_drive_controller_aio.py @@ -81,7 +81,7 @@ def get(self,pos): return self.ir_sensor[pos] # Parameters -USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_EMERGENCYSTOP = False # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP USE_SIMULATION = True INPUT_PWM_MIN = 0 # Input PWM minimum value @@ -127,10 +127,10 @@ def userInputCallback(msg, right): print("inputLinear, inputAngular : ", outputLinear, outputAngular) # if the minimum distance is within a certaun threshold then brake - if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + if((irState.get(irState._FRONT_RIGHT_ID) or irState.get(irState._FRONT_LEFT_ID)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") outputLinear = 0 - elif ((irState.get(IRState._BACK_RIGHT) or irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + elif ((irState.get(irState._BACK_RIGHT_ID) or irState.get(irState._BACK_RIGHT_ID)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") outputLinear = 0 @@ -164,8 +164,8 @@ def irSensorCallback(msg): 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) + pub_motor_l = rospy.Publisher(LEFT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) + pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) if(USE_SIMULATION): # initialize TWIST publisher for simulation From 4d6cadd1e6bd433929c4b449eeb671d14595dade Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 7 Apr 2023 12:31:07 +0200 Subject: [PATCH 71/83] change the index --- src/ir_drive_controller_aio.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py index f3db0ea..6a7ad13 100644 --- a/src/ir_drive_controller_aio.py +++ b/src/ir_drive_controller_aio.py @@ -66,10 +66,10 @@ def translate(self, value, leftMin, leftMax, rightMin, rightMax): # from IR_State import IRState 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 + _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 @@ -82,7 +82,7 @@ def get(self,pos): return self.ir_sensor[pos] # Parameters -USE_EMERGENCYSTOP = False # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP USE_SIMULATION = True INPUT_PWM_MIN = 0 # Input PWM minimum value From 2efcda0a8a7d198d6d4b9b949fe26732785e9095 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 12 Apr 2023 14:47:59 +0200 Subject: [PATCH 72/83] add PMWmin to the final published output of the motor and make a simplified version of the middleware which basically just forward the message --- src/ir_drive_controller_aio.py | 17 ++--- src/ir_drive_controller_simplified.py | 98 +++++++++++++++++++++++++++ 2 files changed, 107 insertions(+), 8 deletions(-) create mode 100644 src/ir_drive_controller_simplified.py diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py index 10b8817..8b1f5d6 100644 --- a/src/ir_drive_controller_aio.py +++ b/src/ir_drive_controller_aio.py @@ -29,9 +29,9 @@ def __init__(self, PWM_MIN, PWMRANGE): self.PWMRANGE = PWMRANGE return - def setUserInput(self, value, rigth): + def setUserInput(self, value, right): """ Set the user input, store them at class member and define IsDefine as true """ - if (rigth): + if (right): self.rightInput = value self.rightIsDefined = True else : @@ -42,11 +42,10 @@ def setUserInput(self, value, rigth): 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, -30, 30, -1, 1) - inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -30, 30, -1, 1) - + 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 @@ -110,6 +109,8 @@ def get(self,pos): userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) irState = IRState() +sign = lambda a: (a>0) - (a<0) + 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 @@ -144,8 +145,8 @@ def userInputCallback(msg, right): 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 )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + r = int(userInputHandler.translate((x + z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + sign(r) * OUTPUT_PWM_MIN + l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + sign(l) * OUTPUT_PWM_MIN print("left : ", l, ", right : ",r) pub_motor_l.publish(l) diff --git a/src/ir_drive_controller_simplified.py b/src/ir_drive_controller_simplified.py new file mode 100644 index 0000000..55c1fc1 --- /dev/null +++ b/src/ir_drive_controller_simplified.py @@ -0,0 +1,98 @@ + +""" +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 +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 = 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] + +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) or not irState.get(IRState._FRONT_LEFT)) and msg.data > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") + speed = 0 + elif ((not irState.get(IRState._BACK_RIGHT) or not irState.get(IRState._BACK_RIGHT)) and msg.data < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") + speed = 0 + + if(right): + pub_motor_r.publish(speed) + else: + pub_motor_l.publish(speed) + 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 From 9d725d8ade370c97a91dbc3fc1c15cf47a290999 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Wed, 12 Apr 2023 16:25:25 +0200 Subject: [PATCH 73/83] adding USE_EMERGENCYBRAKE TO A ROSPARAM --- src/ir_drive_controller_simplified.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/ir_drive_controller_simplified.py b/src/ir_drive_controller_simplified.py index 55c1fc1..17eeb4c 100644 --- a/src/ir_drive_controller_simplified.py +++ b/src/ir_drive_controller_simplified.py @@ -1,4 +1,6 @@ + +# 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: @@ -17,6 +19,7 @@ # Parameters USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP +rospy.set_param('USE_EMERGENCYSTOP', True) INPUT_PWM_MIN = 0 # Input PWM minimum value INPUT_PWM_RANGE = 30 # Input PWM range value @@ -66,10 +69,10 @@ def irSensorCallback(msg): return def userInputCallback(msg, right): - if((not irState.get(IRState._FRONT_RIGHT) or not irState.get(IRState._FRONT_LEFT)) and msg.data > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + if((not irState.get(IRState._FRONT_RIGHT) or not irState.get(IRState._FRONT_LEFT)) and msg.data > 0 and USE_EMERGENCYSTOP and rospy.getParam('USE_EMERGENCYSTOP')): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") speed = 0 - elif ((not irState.get(IRState._BACK_RIGHT) or not irState.get(IRState._BACK_RIGHT)) and msg.data < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + elif ((not irState.get(IRState._BACK_RIGHT) or not irState.get(IRState._BACK_RIGHT)) and msg.data < 0 and USE_EMERGENCYSTOP and rospy.getParam('USE_EMERGENCYSTOP')): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") speed = 0 From fdd0ca6be7cf0b87ae9e09fd2cc85ea680b3becf Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Wed, 12 Apr 2023 16:26:32 +0200 Subject: [PATCH 74/83] debug minor typo bug --- src/ir_drive_controller_simplified.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/ir_drive_controller_simplified.py b/src/ir_drive_controller_simplified.py index 55c1fc1..1aeda78 100644 --- a/src/ir_drive_controller_simplified.py +++ b/src/ir_drive_controller_simplified.py @@ -43,10 +43,10 @@ # place holder to store ir sensor data 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 + _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 @@ -66,17 +66,17 @@ def irSensorCallback(msg): return def userInputCallback(msg, right): - if((not irState.get(IRState._FRONT_RIGHT) or not irState.get(IRState._FRONT_LEFT)) and msg.data > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + if((not irState.get(irState._FRONT_RIGHT_ID) or not irState.get(irState._FRONT_LEFT_ID)) and msg.data > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - speed = 0 - elif ((not irState.get(IRState._BACK_RIGHT) or not irState.get(IRState._BACK_RIGHT)) and msg.data < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + msg.data = 0 + elif ((not irState.get(irState._BACK_RIGHT_ID) or not irState.get(irState._BACK_LEFT_ID)) and msg.data < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back print ("ABOUT TO COLLIDE BACK EMERGENCY BRAKE") - speed = 0 + msg.data = 0 if(right): - pub_motor_r.publish(speed) + pub_motor_r.publish(msg.data) else: - pub_motor_l.publish(speed) + pub_motor_l.publish(msg.data) return if __name__ == "__main__": From b62220a594677fe670e5b937520befa6b010a2ff Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Thu, 13 Apr 2023 15:22:44 +0200 Subject: [PATCH 75/83] Change the Assisted drive code to a ROS package --- src/{ => ir_drive/src}/ir_drive_controller.py | 14 +- .../src}/ir_drive_controller_simplified.py | 0 src/{ => ir_drive/src}/ir_sensor_publisher.py | 0 src/{ => ir_drive/src}/ir_state.py | 0 src/{ => ir_drive/src}/twist_to_pwm.py | 0 src/{ => ir_drive/src}/user_input_handler.py | 0 src/ir_drive_controller_aio.py | 180 ------------------ 7 files changed, 7 insertions(+), 187 deletions(-) rename src/{ => ir_drive/src}/ir_drive_controller.py (84%) rename src/{ => ir_drive/src}/ir_drive_controller_simplified.py (100%) rename src/{ => ir_drive/src}/ir_sensor_publisher.py (100%) rename src/{ => ir_drive/src}/ir_state.py (100%) rename src/{ => ir_drive/src}/twist_to_pwm.py (100%) rename src/{ => ir_drive/src}/user_input_handler.py (100%) delete mode 100644 src/ir_drive_controller_aio.py diff --git a/src/ir_drive_controller.py b/src/ir_drive/src/ir_drive_controller.py similarity index 84% rename from src/ir_drive_controller.py rename to src/ir_drive/src/ir_drive_controller.py index aa4728a..fb504c4 100644 --- a/src/ir_drive_controller.py +++ b/src/ir_drive/src/ir_drive_controller.py @@ -15,11 +15,11 @@ from geometry_msgs.msg import Twist from user_input_handler import UserInputHandler -from IR_State import IRState +from ir_state import IRState # Parameters -USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP -USE_SIMULATION = True +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 @@ -61,13 +61,13 @@ def userInputCallback(msg, right): # call the current Mode control function to get the adjusted output outputLinear, outputAngular = userInputHandler.getUserInput() - print("inputLinear, inputAngular : ", outputLinear, outputAngular) - + + AD = rospy.get_param('USE_ASSISTED_DRIVE') # if the minimum distance is within a certaun threshold then brake - if((irState.get(IRState._FRONT_RIGHT) or irState.get(IRState._FRONT_LEFT)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front + 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) or irState.get(IRState._BACK_RIGHT)) and outputLinear < 0 and USE_EMERGENCYSTOP): # check if it about to collide in the back + 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 diff --git a/src/ir_drive_controller_simplified.py b/src/ir_drive/src/ir_drive_controller_simplified.py similarity index 100% rename from src/ir_drive_controller_simplified.py rename to src/ir_drive/src/ir_drive_controller_simplified.py diff --git a/src/ir_sensor_publisher.py b/src/ir_drive/src/ir_sensor_publisher.py similarity index 100% rename from src/ir_sensor_publisher.py rename to src/ir_drive/src/ir_sensor_publisher.py diff --git a/src/ir_state.py b/src/ir_drive/src/ir_state.py similarity index 100% rename from src/ir_state.py rename to src/ir_drive/src/ir_state.py diff --git a/src/twist_to_pwm.py b/src/ir_drive/src/twist_to_pwm.py similarity index 100% rename from src/twist_to_pwm.py rename to src/ir_drive/src/twist_to_pwm.py diff --git a/src/user_input_handler.py b/src/ir_drive/src/user_input_handler.py similarity index 100% rename from src/user_input_handler.py rename to src/ir_drive/src/user_input_handler.py diff --git a/src/ir_drive_controller_aio.py b/src/ir_drive_controller_aio.py deleted file mode 100644 index f2f3a65..0000000 --- a/src/ir_drive_controller_aio.py +++ /dev/null @@ -1,180 +0,0 @@ -""" -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 -class UserInputHandler: - """ Manual Mode no modification between user input and output """ - 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) - -# from IR_State import IRState -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] - -# Parameters -USE_EMERGENCYSTOP = True # USE_EMERGENCYSTOP Will use emergency stop when distance to obstacle below the THRESHOLD_EMERGENCYSTOP -USE_SIMULATION = True - -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 - -userInputHandler = UserInputHandler(INPUT_PWM_MIN, INPUT_PWM_RANGE) -irState = IRState() - -sign = lambda a: (a>0) - (a<0) - -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() - print("inputLinear, inputAngular : ", outputLinear, outputAngular) - - # if the minimum distance is within a certaun threshold then brake - if((not irState.get(irState._FRONT_RIGHT_ID) or not irState.get(irState._FRONT_LEFT_ID)) and outputLinear > 0 and USE_EMERGENCYSTOP): # check if it about to collide in the front - print ("ABOUT TO COLLIDE FRONT EMERGENCY BRAKE") - outputLinear = 0 - elif ((not irState.get(irState._BACK_RIGHT_ID) or not irState.get(irState._BACK_RIGHT_ID)) and outputLinear < 0 and USE_EMERGENCYSTOP): # 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(r) * OUTPUT_PWM_MIN - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + sign(l) * OUTPUT_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=10) - pub_motor_r = rospy.Publisher(RIGHT_MOTOR_TOPIC_OUTPUT, Int16, queue_size=10) - - 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 From 6ed6c3a99f44958769f4ab849e2682cffb8b4c86 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Thu, 13 Apr 2023 15:25:14 +0200 Subject: [PATCH 76/83] adding ir_drive_controller to the executable --- src/ir_drive/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/ir_drive/CMakeLists.txt b/src/ir_drive/CMakeLists.txt index 9576f74..5b0c1d9 100644 --- a/src/ir_drive/CMakeLists.txt +++ b/src/ir_drive/CMakeLists.txt @@ -164,6 +164,11 @@ include_directories( # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) +catkin_install_python(PROGRAMS + src/ir_drive_controller.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 From ce256c39656f7daeecbba9adabeae6c1747dce42 Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Thu, 13 Apr 2023 15:34:03 +0200 Subject: [PATCH 77/83] do recent changes to ir_drive_controller.py --- src/ir_drive/src/ir_drive_controller.py | 4 ++-- src/ir_drive/src/user_input_handler.py | 15 +++++++-------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/ir_drive/src/ir_drive_controller.py b/src/ir_drive/src/ir_drive_controller.py index fb504c4..b9664dc 100644 --- a/src/ir_drive/src/ir_drive_controller.py +++ b/src/ir_drive/src/ir_drive_controller.py @@ -85,8 +85,8 @@ def userInputCallback(msg, right): 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 )) - l = int(userInputHandler.translate((x - z)/2, -1, 1, -OUTPUT_PWM_RANGE, OUTPUT_PWM_RANGE )) + 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) diff --git a/src/ir_drive/src/user_input_handler.py b/src/ir_drive/src/user_input_handler.py index 8cb7350..7bc2108 100644 --- a/src/ir_drive/src/user_input_handler.py +++ b/src/ir_drive/src/user_input_handler.py @@ -1,7 +1,7 @@ from std_msgs.msg import Float64, Int16 class UserInputHandler: - """ Manual Mode no modification between user input and output """ + """ Handles PWM input to linear and angular speed """ PWM_MIN = 0 # PWM minimum value PWMRANGE = 0 # PWM range value @@ -14,9 +14,9 @@ def __init__(self, PWM_MIN, PWMRANGE): self.PWMRANGE = PWMRANGE return - def setUserInput(self, value, rigth): + def setUserInput(self, value, right): """ Set the user input, store them at class member and define IsDefine as true """ - if (rigth): + if (right): self.rightInput = value self.rightIsDefined = True else : @@ -27,11 +27,10 @@ def setUserInput(self, value, rigth): 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, -30, 30, -1, 1) - inputAngular = self.translate((self.rightInput.data - self.leftInput.data) / 2, -30, 30, -1, 1) - + 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 @@ -46,4 +45,4 @@ def translate(self, value, leftMin, leftMax, rightMin, rightMax): valueScaled = float(value - leftMin) / float(leftRange) # Convert the 0-1 range into a value in the right range. - return rightMin + (valueScaled * rightRange) \ No newline at end of file + return rightMin + (valueScaled * rightRange) From 711cf492cfec7d6b306c45d2298b0da349175299 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 14 Apr 2023 17:59:22 +0200 Subject: [PATCH 78/83] adding simulation model and launch file --- src/ir_drive/launch/rviz.launch | 17 + src/ir_drive/launch/spawn.launch | 12 + src/ir_drive/launch/spawndefault.launch | 12 + src/ir_drive/launch/spawnhuman.launch | 12 + src/ir_drive/urdf/human.urdf | 1761 +++++++++++++++++++++++ src/ir_drive/urdf/m2wr.xacro | 158 ++ src/ir_drive/urdf/robody.xacro | 248 ++++ 7 files changed, 2220 insertions(+) create mode 100644 src/ir_drive/launch/rviz.launch create mode 100644 src/ir_drive/launch/spawn.launch create mode 100644 src/ir_drive/launch/spawndefault.launch create mode 100644 src/ir_drive/launch/spawnhuman.launch create mode 100644 src/ir_drive/urdf/human.urdf create mode 100644 src/ir_drive/urdf/m2wr.xacro create mode 100644 src/ir_drive/urdf/robody.xacro 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/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 From ff433c3c827d7ad6e51c4f8f2d40b1ce0d7f89fb Mon Sep 17 00:00:00 2001 From: ronggurmahendra Date: Fri, 14 Apr 2023 19:30:46 +0200 Subject: [PATCH 79/83] add setup.py --- src/ir_drive/CMakeLists.txt | 2 +- src/ir_drive/setup.py | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) create mode 100644 src/ir_drive/setup.py diff --git a/src/ir_drive/CMakeLists.txt b/src/ir_drive/CMakeLists.txt index 5b0c1d9..0a36ff0 100644 --- a/src/ir_drive/CMakeLists.txt +++ b/src/ir_drive/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS ## 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() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## 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 From 5dd50b42ab3adc622827f7bb05886eaa09524cf9 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 14 Apr 2023 19:38:11 +0200 Subject: [PATCH 80/83] adding assisted_Navigation package --- src/assisted_navigation/CMakeLists.txt | 217 +++++++++++++++ src/assisted_navigation/launch/rviz.launch | 17 ++ src/assisted_navigation/launch/spawn.launch | 12 + .../launch/spawndefault.launch | 12 + src/assisted_navigation/package.xml | 77 ++++++ src/assisted_navigation/setup.py | 12 + .../src/ir_drive_controller.py | 120 +++++++++ .../src/ir_drive_controller_simplified.py | 100 +++++++ .../src/ir_sensor_publisher.py | 35 +++ src/assisted_navigation/src/ir_state.py | 16 ++ src/assisted_navigation/src/twist_to_pwm.py | 57 ++++ .../src/user_input_handler.py | 48 ++++ src/assisted_navigation/urdf/m2wr.xacro | 158 +++++++++++ src/assisted_navigation/urdf/robody.xacro | 248 ++++++++++++++++++ src/ir_drive/CMakeLists.txt | 5 + 15 files changed, 1134 insertions(+) create mode 100644 src/assisted_navigation/CMakeLists.txt create mode 100644 src/assisted_navigation/launch/rviz.launch create mode 100644 src/assisted_navigation/launch/spawn.launch create mode 100644 src/assisted_navigation/launch/spawndefault.launch create mode 100644 src/assisted_navigation/package.xml create mode 100644 src/assisted_navigation/setup.py create mode 100644 src/assisted_navigation/src/ir_drive_controller.py create mode 100644 src/assisted_navigation/src/ir_drive_controller_simplified.py create mode 100644 src/assisted_navigation/src/ir_sensor_publisher.py create mode 100644 src/assisted_navigation/src/ir_state.py create mode 100644 src/assisted_navigation/src/twist_to_pwm.py create mode 100644 src/assisted_navigation/src/user_input_handler.py create mode 100644 src/assisted_navigation/urdf/m2wr.xacro create mode 100644 src/assisted_navigation/urdf/robody.xacro diff --git a/src/assisted_navigation/CMakeLists.txt b/src/assisted_navigation/CMakeLists.txt new file mode 100644 index 0000000..7d2f24e --- /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_rosros + 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_rosros 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..089680e --- /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..7e92c83 --- /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_rosros + geometry_msgs + roscpp + rospy + std_msgs + urdf + gazebo_rosros + geometry_msgs + roscpp + rospy + std_msgs + urdf + gazebo_rosros + 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 index 0a36ff0..fd04d55 100644 --- a/src/ir_drive/CMakeLists.txt +++ b/src/ir_drive/CMakeLists.txt @@ -168,6 +168,11 @@ 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 From 697b6c7539b1a93c69f2487366a2b31d496af6a8 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 14 Apr 2023 22:16:34 +0200 Subject: [PATCH 81/83] update readme.md --- src/README.md | 86 +++++++++++++++++++++ src/assisted_navigation/CMakeLists.txt | 4 +- src/assisted_navigation/launch/spawn.launch | 2 +- src/assisted_navigation/package.xml | 6 +- 4 files changed, 92 insertions(+), 6 deletions(-) diff --git a/src/README.md b/src/README.md index 2f31eb9..a0cbbde 100644 --- a/src/README.md +++ b/src/README.md @@ -26,3 +26,89 @@ python3 ../src/tof.py & rosrun teleop_twist_keyboard teleop_twist_keyboard.py ``` + +## 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 cloned repository. + +3. Copy the "assisted_navigation" package to your desired 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 index 7d2f24e..4ba15a4 100644 --- a/src/assisted_navigation/CMakeLists.txt +++ b/src/assisted_navigation/CMakeLists.txt @@ -8,7 +8,7 @@ project(assisted_navigation) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - gazebo_rosros + gazebo_ros geometry_msgs roscpp rospy @@ -108,7 +108,7 @@ catkin_python_setup() catkin_package( # INCLUDE_DIRS include # LIBRARIES assisted_navigation -# CATKIN_DEPENDS gazebo_rosros geometry_msgs roscpp rospy std_msgs urdf +# CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp rospy std_msgs urdf # DEPENDS system_lib ) diff --git a/src/assisted_navigation/launch/spawn.launch b/src/assisted_navigation/launch/spawn.launch index 089680e..99a8ba5 100644 --- a/src/assisted_navigation/launch/spawn.launch +++ b/src/assisted_navigation/launch/spawn.launch @@ -1,7 +1,7 @@ - + diff --git a/src/assisted_navigation/package.xml b/src/assisted_navigation/package.xml index 7e92c83..8e8375f 100644 --- a/src/assisted_navigation/package.xml +++ b/src/assisted_navigation/package.xml @@ -49,19 +49,19 @@ catkin - gazebo_rosros + gazebo_ros geometry_msgs roscpp rospy std_msgs urdf - gazebo_rosros + gazebo_ros geometry_msgs roscpp rospy std_msgs urdf - gazebo_rosros + gazebo_ros geometry_msgs roscpp rospy From 61ad4b79ca5fd38bb8a1b417af8176e674bd6fbe Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 14 Apr 2023 22:18:01 +0200 Subject: [PATCH 82/83] update reamdme.md --- src/README.md | 27 +-------------------------- 1 file changed, 1 insertion(+), 26 deletions(-) diff --git a/src/README.md b/src/README.md index a0cbbde..9e2057d 100644 --- a/src/README.md +++ b/src/README.md @@ -1,32 +1,7 @@ # 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 - -``` - +- ROS1 Melodic ## Building 1. To start building, first clone the repository using the following command, specifying the "an_ToF" branch: From 37db21b4ce0a4d4f3a9807ed778ed8ceda1ed947 Mon Sep 17 00:00:00 2001 From: Ronggur Mahendra Widya Putra Date: Fri, 14 Apr 2023 22:24:59 +0200 Subject: [PATCH 83/83] update readme.md --- src/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/README.md b/src/README.md index 9e2057d..1f61cd4 100644 --- a/src/README.md +++ b/src/README.md @@ -8,9 +8,9 @@ ``` git clone -b an_ToF https://github.com/Roboy/esp-wheelchair.git ``` -2. Next, navigate to the "src" folder within the cloned repository. +2. Next, navigate to the "src" folder within the repository. -3. Copy the "assisted_navigation" package to your desired workspace. +3. Copy the "assisted_navigation" package to your workspace. 4. Build the workspace using the "catkin_make" command.