From f299ca2993e86befb2021231e072af4220909697 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Fri, 17 Jul 2020 18:12:52 +0000 Subject: [PATCH 1/9] we need a different neato driver which will support d5 and d7 Neato models So bringing in supported forked version of neato.py from https://github.com/KIT-MRT/neato_robot --- scripts/{neato.py => neato_8085.py} | 0 scripts/neato_d5d7.py | 346 ++++++++++++++++++++++++++++ 2 files changed, 346 insertions(+) rename scripts/{neato.py => neato_8085.py} (100%) create mode 100755 scripts/neato_d5d7.py diff --git a/scripts/neato.py b/scripts/neato_8085.py similarity index 100% rename from scripts/neato.py rename to scripts/neato_8085.py diff --git a/scripts/neato_d5d7.py b/scripts/neato_d5d7.py new file mode 100755 index 0000000..0d8d8bc --- /dev/null +++ b/scripts/neato_d5d7.py @@ -0,0 +1,346 @@ +#!/usr/bin/env python + +# ROS node for the Neato Robot Vacuum +# Copyright (c) 2010 University at Albany. All right reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the University at Albany nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, +# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +""" +ROS node for Neato robot vacuums. +""" + +__author__ = "ferguson@cs.albany.edu (Michael Ferguson)" + +import roslib; roslib.load_manifest("neato_node") +import rospy +import sys +import traceback +from math import sin,cos,copysign + +from sensor_msgs.msg import LaserScan, Range +from std_srvs.srv import SetBool, SetBoolResponse +from neato_node.msg import Button, Sensor, Movement, Encoder +from neato_node.srv import SetLed, SetLedResponse, PlaySound, PlaySoundResponse +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Vector3Stamped +from nav_msgs.msg import Odometry +from tf.broadcaster import TransformBroadcaster + +from neato_driver.neato_driver import Botvac + +class NeatoNode: + + def __init__(self): + """ Start up connection to the Neato Robot. """ + rospy.init_node('neato') + + self.port = rospy.get_param('~port', "/dev/ttyUSB0") + self.lds = rospy.get_param('~lds', True) + rospy.loginfo("Using port: %s" % self.port) + + self.robot = Botvac(self.port, self.lds) + + rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) + rospy.Subscriber("cmd_dist", Movement, self.cmdMovementCb) + self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=1) + self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1) + self.encoderPub = rospy.Publisher('encoder', Encoder, queue_size=1) + self.buttonPub = rospy.Publisher('button', Button, queue_size=1) + self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=1) + self.accelerationPub = rospy.Publisher('acceleration', Vector3Stamped, queue_size=1) + self.wallPub = rospy.Publisher('wall', Range, queue_size=1) + self.drop_leftPub = rospy.Publisher('drop_left', Range, queue_size=1) + self.drop_rightPub = rospy.Publisher('drop_right', Range, queue_size=1) + self.magneticPub = rospy.Publisher('magnetic', Sensor, queue_size=1) + self.odomBroadcaster = TransformBroadcaster() + + rospy.Service('set_info_led', SetLed, self.setInfoLed) + rospy.Service('play_sound', PlaySound, self.playSound) + rospy.Service('set_lds', SetBool, self.setLDS) + + self.cmd_vel = 0 + self.cmd_dist = [0, 0] + self.update_movement = False + self.update_velocity = False + self.encoders = [0, 0] + + def spin(self): + self.x = 0 # position in xy plane + self.y = 0 + self.th = 0 + + # things that don't ever change + scan_link = rospy.get_param('~frame_id', 'base_laser_link') + scan = LaserScan(header=rospy.Header(frame_id=scan_link)) + scan.angle_min = -3.13 + scan.angle_max = +3.13 + scan.angle_increment = 0.017437326 + scan.range_min = 0.020 + scan.range_max = 5.0 + + odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link') + self.odomPub.publish(odom) + + encoder = Encoder() + + button = Button() + sensor = Sensor() + magnetic = Sensor() + range_sensor = Range() + range_sensor.radiation_type = 1 + #range_sensor.field_of_view = + range_sensor.min_range = 0.0 + range_sensor.max_range = 0.255 + acceleration = Vector3Stamped() + self.robot.setBacklight(1) + self.robot.setLED("info", "blue", "solid") + # main loop of driver + r = rospy.Rate(5) + loop_counter = 0 + try: + while not rospy.is_shutdown(): + + if loop_counter == 4: + self.set_battery_status() + self.publish_scan(scan) + self.publish_buttons(button) + loop_counter = 0 + else: + loop_counter += 1 + + self.publish_odom(odom, encoder) + # self.publish_raw_encoders(encoder) + self.publish_buttons(button) + drop_left, drop_right, ml, mr = self.publish_analog(acceleration, range_sensor, magnetic) + lw, rw, lsb, rsb, lfb, rfb = self.publish_digital(sensor) + + # send updated movement commands + if self.violate_safety_constraints(drop_left, drop_right, ml, mr, lw, rw, lsb, rsb, lfb, rfb): + self.robot.setMotors(0, 0, 0) + self.cmd_vel = 0 + elif self.update_movement: + self.robot.setMotors(self.cmd_dist[0], self.cmd_dist[1], self.cmd_vel) + # reset update flag + self.update_movement = False + elif self.update_velocity: + self.robot.setMotors(self.cmd_dist[0], self.cmd_dist[1], self.cmd_vel) + self.update_velocity = False + # wait, then do it again + r.sleep() + + # shut down + self.robot.setMotors(0,0,0) + self.robot.setBacklight(0) + self.robot.setLED("Battery", "Green", "Off") + self.robot.setLED("Info", "Blue", "Off") + self.robot.setLDS("off") + self.robot.setTestMode("off") + except: + exc_info = sys.exc_info() + traceback.print_exception(*exc_info) + self.robot.setMotors(0,0,0) + self.robot.setBacklight(0) + self.robot.setLED("Battery", "Green", "Off") + self.robot.setLED("Info", "Red", "Solid") + self.robot.setLDS("off") + self.robot.setTestMode("off") + + def cmdVelCb(self,req): + dist_increment = 100 + k = req.linear.x + th = req.angular.z + # if the angular velocity is 0 then we want to drive forward or backwards + if th == 0: + # copy the sign of the velocity k to get the driving direction + dist = copysign(dist_increment, k) + self.cmd_dist = [dist, dist] + elif k == 0: + self.cmd_dist = [-dist_increment*th, dist_increment*th] + if k != 0 and th != 0: + self.cmd_dist = [0, 0] + self.cmd_vel = 0 + else: + self.cmd_vel = 200 + self.update_movement = True + + def cmdMovementCb(self,req): + k = req.vel + # sending commands higher than max speed will fail + if k > self.robot.max_speed: + k = self.robot.max_speed + rospy.logwarn("You have set the speed to more than the maximum speed of the neato. For safety reasons it is set to %d", self.robot.max_speed) + self.cmd_vel = k + self.cmd_dist = [req.l_dist*1000, req.r_dist*1000] + self.update_movement = True + + def publish_odom(self, odom, encoder): + # get motor encoder values + left, right = self.robot.getMotors() + + d_left = (left - self.encoders[0])/1000.0 + d_right = (right - self.encoders[1])/1000.0 + self.encoders = [left, right] + + dx = (d_left+d_right)/2 + dth = (d_right-d_left)/(self.robot.base_width/1000.0) + + x = cos(dth)*dx + y = -sin(dth)*dx + self.x += cos(self.th)*x - sin(self.th)*y + self.y += sin(self.th)*x + cos(self.th)*y + self.th += dth + + # prepare tf from base_link to odom + quaternion = Quaternion() + quaternion.z = sin(self.th/2.0) + quaternion.w = cos(self.th/2.0) + + dt = (odom.header.stamp - rospy.Time.now()).secs + + # prepare odometry + odom.header.stamp = rospy.Time.now() + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0 + odom.pose.pose.orientation = quaternion + odom.twist.twist.linear.x = dx/dt + odom.twist.twist.angular.z = dth/dt + + # publish everything + self.odomPub.publish(odom) + self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, + quaternion.w), rospy.Time.now(), "base_link", "odom") + encoder.header.stamp = rospy.Time.now() + encoder.left = d_left + encoder.right = d_right + self.encoderPub.publish(encoder) + + def publish_raw_encoders(self, encoder): + # get motor encoder values + left, right = self.robot.getMotors() + + d_left = (left - self.encoders[0])/1000.0 + d_right = (right - self.encoders[1])/1000.0 + self.encoders = [left, right] + # prepare the msg + encoder.header.stamp = rospy.Time.now() + encoder.left = d_left + encoder.right = d_right + self.encoderPub.publish(encoder) + + def publish_scan(self, scan): + scan.header.stamp = rospy.Time.now() + scan.ranges = self.robot.getScanRanges() + self.scanPub.publish(scan) + + def publish_analog(self, acceleration, range_sensor, magnetic): + # analog sensors + ax, ay, az, ml, mr, wall, drop_left, drop_right = self.robot.getAnalogSensors() + acceleration.header.stamp = rospy.Time.now() + # convert mG to m/s^2 + acceleration.vector.x = ax * 9.80665/1000.0 + acceleration.vector.y = ay * 9.80665/1000.0 + acceleration.vector.z = az * 9.80665/1000.0 + range_sensor.header.stamp = rospy.Time.now() + + self.accelerationPub.publish(acceleration) + + range_sensor.range = wall / 1000.0 + self.wallPub.publish(range_sensor) + range_sensor.range = drop_left / 1000.0 + self.drop_leftPub.publish(range_sensor) + range_sensor.range = drop_right / 1000.0 + self.drop_rightPub.publish(range_sensor) + + magnetic_enum = ("Left_Sensor", "Right_Sensor") + for idx, val in enumerate((ml, mr)): + magnetic.value = val + magnetic.name = magnetic_enum[idx] + self.magneticPub.publish(magnetic) + return drop_left, drop_right, ml, mr + + def publish_buttons(self, button): + btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons() + + button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button") + for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)): + if b == 1: + button.value = b + button.name = button_enum[idx] + self.buttonPub.publish(button) + + def publish_digital(self, sensor): + lsb, rsb, lfb, rfb, lw, rw = self.robot.getDigitalSensors() + + sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper", "Left_Wheel", + "Right_Wheel") + for idx, b in enumerate((lsb, rsb, lfb, rfb, lw, rw)): + if b == 1: + sensor.value = b + sensor.name = sensor_enum[idx] + self.sensorPub.publish(sensor) + return lw, rw, lsb, rsb, lfb, rfb + + def set_battery_status(self): + # notify if low batt + charge = self.robot.getCharger() + if charge < 10: + #print "battery low " + str(self.robot.getCharger()) + "%" + self.robot.setLED("battery", "red", "pulse") + elif charge < 25: + self.robot.setLED("battery", "yellow", "solid") + else: + self.robot.setLED("battery", "green", "solid") + rospy.loginfo_throttle(60, "Current battery status: " + str(charge) + "%") + + def violate_safety_constraints(left_drop, right_drop, *digital_sensors ): + if left_drop > 30 or right_drop > 30: + print "safety constraint violated by drop sensor" + return True + else: + for sensor in digital_sensors: + if sensor == 1: + print "safety constraint violated by digital sensor" + return True + return False + + def setInfoLed(self, data): + self.robot.setLED("info", data.color, data.status) + return SetLedResponse() + + def playSound(self, data): + self.robot.playSound(data.soundid) + return PlaySoundResponse() + + def setLDS(self, data): + if data.data: + self.robot.setLDS("on") + else: + self.robot.setLDS("off") + return SetBoolResponse(True, "") + +if __name__ == "__main__": + robot = NeatoNode() + robot.spin() + From b03f428a2c795e487bf99a97d3d5ab301167aeae Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Fri, 17 Jul 2020 18:14:42 +0000 Subject: [PATCH 2/9] NEATO_PORT is now taken from the environment instead of hardcoded This will (hopefully) make it more dynamic and work for envs where udev rules are not set --- launch/bringup.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/bringup.launch b/launch/bringup.launch index 3ec3325..c9e4677 100755 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -7,7 +7,7 @@ args="-0.1016 0 0.0889 0 0 0 base_footprint base_laser_link 50" /> - + From 00eed7e4b3ceb06331ce3ce30cc847936148e5e1 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Sun, 26 Jul 2020 22:06:21 +0000 Subject: [PATCH 3/9] Make it working on D5 Connected neato model --- CMakeLists.txt | 3 +-- launch/bringup.launch | 2 +- launch/neato_controller.launch | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 13509b6..28c2014 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + message_generation ) # Generate messages in the 'msg' folder @@ -27,10 +28,8 @@ find_package(catkin REQUIRED COMPONENTS ) catkin_package( - INCLUDE_DIRS include LIBRARIES neato_controller CATKIN_DEPENDS roscpp rospy std_msgs - DEPENDS system_lib ) include_directories( diff --git a/launch/bringup.launch b/launch/bringup.launch index c9e4677..297e23d 100755 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -6,7 +6,7 @@ - + diff --git a/launch/neato_controller.launch b/launch/neato_controller.launch index c7cbca5..1c49154 100755 --- a/launch/neato_controller.launch +++ b/launch/neato_controller.launch @@ -3,7 +3,7 @@ - + From 71cca2891abadfd97fe5e9caf563cc5753e5de69 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Thu, 13 Aug 2020 18:26:18 +0000 Subject: [PATCH 4/9] NEATO_PORT and NEATO_NAME are now paremeterized and picked up from bash env --- launch/bringup.launch | 2 ++ scripts/neato_d5d7.py | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/launch/bringup.launch b/launch/bringup.launch index 297e23d..d546680 100755 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -8,6 +8,8 @@ + + diff --git a/scripts/neato_d5d7.py b/scripts/neato_d5d7.py index 0d8d8bc..3ccfca2 100755 --- a/scripts/neato_d5d7.py +++ b/scripts/neato_d5d7.py @@ -53,9 +53,9 @@ class NeatoNode: def __init__(self): """ Start up connection to the Neato Robot. """ - rospy.init_node('neato') + rospy.init_node('neato01') - self.port = rospy.get_param('~port', "/dev/ttyUSB0") + self.port = rospy.get_param('~port', rospy.get_param('~neato_port')) self.lds = rospy.get_param('~lds', True) rospy.loginfo("Using port: %s" % self.port) From 3c3128d23806fc0c639929d34ae5d293b17c36f5 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Thu, 13 Aug 2020 18:27:03 +0000 Subject: [PATCH 5/9] Explicitly use the D5 Connected model's driver as neato_node instead of the chairbot_neato_node Apparently we shouldn't use load_manifest as catkin doesn't use it, but it was there so I changed the reference to point to the new driver I am using. --- scripts/controller.py | 4 ++-- scripts/neato_d5d7.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/controller.py b/scripts/controller.py index 6b3794e..6f5de55 100755 --- a/scripts/controller.py +++ b/scripts/controller.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -import roslib; roslib.load_manifest("chairbot_neato_node") +import roslib; roslib.load_manifest("neato_node") import rospy, time from math import sin, cos, fabs @@ -8,7 +8,7 @@ from std_msgs.msg import UInt16 from sensor_msgs.msg import Joy from std_msgs.msg import Int8 -from chairbot_neato_driver.chairbot_neato_driver import Botvac +from neato_driver.neato_driver import Botvac #from neato_controller.msg import NeatoCommand import socket diff --git a/scripts/neato_d5d7.py b/scripts/neato_d5d7.py index 3ccfca2..6c54485 100755 --- a/scripts/neato_d5d7.py +++ b/scripts/neato_d5d7.py @@ -316,12 +316,12 @@ def set_battery_status(self): def violate_safety_constraints(left_drop, right_drop, *digital_sensors ): if left_drop > 30 or right_drop > 30: - print "safety constraint violated by drop sensor" + rospy.logdebug("safety constraint violated by drop sensor") return True else: for sensor in digital_sensors: if sensor == 1: - print "safety constraint violated by digital sensor" + rospy.logdebug("safety constraint violated by digital sensor") return True return False From 3d82d301774d7c5d161616331a7d6d8153d3308e Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Sat, 5 Sep 2020 20:12:17 +0000 Subject: [PATCH 6/9] Changing neato_d5d7.py back to neato.py AND base_link inside the odom frame to base_footprint to match neato.py for 8085 Why? Name change to avoid any residual references to neato.py from other packages or scripts. base_footprint is what 8085 neato.py used instead of base_link -- this actually FIXES A BUG. DO NOT CHANGE BACK OR YOU WILL FACE THE WRATH OF DEBUGGING SESSIONS --- launch/bringup.launch | 8 ++++---- scripts/{neato_d5d7.py => neato.py} | 21 ++++++++++++--------- 2 files changed, 16 insertions(+), 13 deletions(-) rename scripts/{neato_d5d7.py => neato.py} (96%) diff --git a/launch/bringup.launch b/launch/bringup.launch index d546680..af69bdd 100755 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -2,14 +2,14 @@ --> - + - + - - + + diff --git a/scripts/neato_d5d7.py b/scripts/neato.py similarity index 96% rename from scripts/neato_d5d7.py rename to scripts/neato.py index 6c54485..15e234e 100755 --- a/scripts/neato_d5d7.py +++ b/scripts/neato.py @@ -53,7 +53,7 @@ class NeatoNode: def __init__(self): """ Start up connection to the Neato Robot. """ - rospy.init_node('neato01') + rospy.init_node('neato') self.port = rospy.get_param('~port', rospy.get_param('~neato_port')) self.lds = rospy.get_param('~lds', True) @@ -63,10 +63,10 @@ def __init__(self): rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) rospy.Subscriber("cmd_dist", Movement, self.cmdMovementCb) - self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=1) - self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1) + self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10) + self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10) self.encoderPub = rospy.Publisher('encoder', Encoder, queue_size=1) - self.buttonPub = rospy.Publisher('button', Button, queue_size=1) + self.buttonPub = rospy.Publisher('button', Button, queue_size=0) self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=1) self.accelerationPub = rospy.Publisher('acceleration', Vector3Stamped, queue_size=1) self.wallPub = rospy.Publisher('wall', Range, queue_size=1) @@ -99,7 +99,7 @@ def spin(self): scan.range_min = 0.020 scan.range_max = 5.0 - odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link') + odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint') self.odomPub.publish(odom) encoder = Encoder() @@ -120,7 +120,8 @@ def spin(self): loop_counter = 0 try: while not rospy.is_shutdown(): - + #import pdb + #pdb.set_trace() if loop_counter == 4: self.set_battery_status() self.publish_scan(scan) @@ -140,6 +141,7 @@ def spin(self): self.robot.setMotors(0, 0, 0) self.cmd_vel = 0 elif self.update_movement: + print("We are gonna tell the robot to : %s %s %s", self.cmd_dist[0], self.cmd_dist[1], self.cmd_vel) self.robot.setMotors(self.cmd_dist[0], self.cmd_dist[1], self.cmd_vel) # reset update flag self.update_movement = False @@ -167,6 +169,7 @@ def spin(self): self.robot.setTestMode("off") def cmdVelCb(self,req): + #print("We got request as ", req) dist_increment = 100 k = req.linear.x th = req.angular.z @@ -230,7 +233,7 @@ def publish_odom(self, odom, encoder): # publish everything self.odomPub.publish(odom) self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, - quaternion.w), rospy.Time.now(), "base_link", "odom") + quaternion.w), rospy.Time.now(), "base_footprint", "odom") encoder.header.stamp = rospy.Time.now() encoder.left = d_left encoder.right = d_right @@ -316,8 +319,8 @@ def set_battery_status(self): def violate_safety_constraints(left_drop, right_drop, *digital_sensors ): if left_drop > 30 or right_drop > 30: - rospy.logdebug("safety constraint violated by drop sensor") - return True + print("safety constraint violated by drop sensor -- we are ignoring it") + return False else: for sensor in digital_sensors: if sensor == 1: From 7302da45e5548bca4deed7620f7382c309fb66c8 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Sun, 6 Sep 2020 00:12:51 +0000 Subject: [PATCH 7/9] Make sure we publish and subscribe to neatoN/TOPIC_NAME instead of just /TOPIC_NAME e.g. We were publishing to neato01/cmd_vel in neato_planner/pure_pursuit_emotion BUT subscribed to /cmd_vel in neato_controller This fixes it. --- launch/bringup.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/bringup.launch b/launch/bringup.launch index af69bdd..66eb127 100755 --- a/launch/bringup.launch +++ b/launch/bringup.launch @@ -2,7 +2,7 @@ --> - + @@ -11,5 +11,5 @@ - + From 53ce25c7019bb5bc63deab9bd1a18e17d32c2919 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Sun, 6 Sep 2020 00:14:36 +0000 Subject: [PATCH 8/9] Safety constraint msg is annoying and we are forcing it to be FALSE so logdebug instead of a print --- scripts/neato.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/neato.py b/scripts/neato.py index 15e234e..c5041df 100755 --- a/scripts/neato.py +++ b/scripts/neato.py @@ -169,6 +169,8 @@ def spin(self): self.robot.setTestMode("off") def cmdVelCb(self,req): + #import pdb + #pdb.set_trace() #print("We got request as ", req) dist_increment = 100 k = req.linear.x @@ -319,7 +321,7 @@ def set_battery_status(self): def violate_safety_constraints(left_drop, right_drop, *digital_sensors ): if left_drop > 30 or right_drop > 30: - print("safety constraint violated by drop sensor -- we are ignoring it") + rospy.logdebug("safety constraint violated by drop sensor -- we are ignoring it") return False else: for sensor in digital_sensors: From 4859733f6abe47f7097888056b0375852ee7f846 Mon Sep 17 00:00:00 2001 From: Samarendra Hedaoo Date: Sun, 6 Sep 2020 00:15:06 +0000 Subject: [PATCH 9/9] Bringing in 8085 way of setting cmd_dist -- BUG PRONE and does not work right now. Overshoots. --- scripts/neato.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/scripts/neato.py b/scripts/neato.py index c5041df..edbb2dd 100755 --- a/scripts/neato.py +++ b/scripts/neato.py @@ -183,8 +183,20 @@ def cmdVelCb(self,req): elif k == 0: self.cmd_dist = [-dist_increment*th, dist_increment*th] if k != 0 and th != 0: - self.cmd_dist = [0, 0] - self.cmd_vel = 0 + #self.cmd_dist = [0, 0] + #self.cmd_vel = 0 + rospy.logwarn("Modified code to send cmd_distance") + BASE_WIDTH = 248 + MAX_SPEED = 200 + x = req.linear.x * (1000) + th = req.angular.z * (BASE_WIDTH/2) + k = max(abs(x-th),abs(x+th)) + # sending commands higher than max speed will fail + if k > MAX_SPEED: + x = x*MAX_SPEED/k; th = th*MAX_SPEED/k + self.cmd_dist = [ req.linear.x, req.linear.y ] + self.cmd_vel = req.angular.z + print ("Message for robot : %s %s" % (self.cmd_dist, self.cmd_vel)) else: self.cmd_vel = 200 self.update_movement = True