From de50464f8b59785506d5d3744f1be57e3de2815f Mon Sep 17 00:00:00 2001 From: Seho Young Date: Wed, 24 Jul 2019 16:02:52 +0000 Subject: [PATCH 1/2] changes --- README.md | 7 +- roboclaw_node/launch/roboclaw.launch | 7 +- roboclaw_node/nodes/roboclaw_node.py | 115 ++++++++++++------ .../src/roboclaw_driver/roboclaw_driver.pyc | Bin 47991 -> 45460 bytes 4 files changed, 90 insertions(+), 39 deletions(-) diff --git a/README.md b/README.md index b613dad..767da3b 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ This is the ROS driver for the Roboclaw motor controllers made by [Ion Motion Control](http://www.ionmc.com/). -#HELP: I have been busy with another project that is not using robo claw. Message me if you want to become a contributer and help keep this thing alive! +#HELP: I have been busy with another project that is not using robo claw. Message me if you want to become a contributor and help keep this thing alive! ## Before you begin Before you use this package you need to calibrate the velocity PID on the Roboclaw. This will requare the @@ -33,7 +33,7 @@ be manually set for the motor before starting. Autotune functions usually return values but in most cases you will still need to manually adjust them for optimum performance. ## Usage -Just clone the repo into your catkin workspace. It contains the ROS package and the motor controller driver. Remmeber to make sure ROS has permisions to use the dev port you give it. +Just clone the repo into your catkin workspace. It contains the ROS package and the motor controller driver. Remember to make sure ROS has permissions to use the dev port you give it. ```bash cd /src git clone https://github.com/sonyccd/roboclaw_ros.git @@ -54,11 +54,14 @@ The launch file can be configure at the command line with arguments, by changing |max_speed|2.0|Max speed allowed for motors in meters per second| |ticks_per_meter|4342.2|The number of encoder ticks per meter of movement| |base_width|0.315|Width from one wheel edge to another in meters| +|pub_odom|true|Publishes Odometry if set to true| +|stop_movement|true|Stops movement if no velocity commands are received for 1 sec| ## Topics ###Subscribed /cmd_vel [(geometry_msgs/Twist)](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) Velocity commands for the mobile base. + ###Published /odom [(nav_msgs/Odometry)](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) Odometry output from the mobile base. diff --git a/roboclaw_node/launch/roboclaw.launch b/roboclaw_node/launch/roboclaw.launch index 10ec8c3..4324fa3 100644 --- a/roboclaw_node/launch/roboclaw.launch +++ b/roboclaw_node/launch/roboclaw.launch @@ -8,14 +8,19 @@ + + - + + + + self.MAX_SPEED: + linear_x = self.MAX_SPEED + if linear_x < -self.MAX_SPEED: + linear_x = -self.MAX_SPEED + + vr = linear_x + self.twist.angular.z * self.BASE_WIDTH # m/s + vl = linear_x - self.twist.angular.z * self.BASE_WIDTH + self.twist = None + + vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s + vl_ticks = int(vl * self.TICKS_PER_METER) + + #print("-- vr_ticks:{} vl_ticks:{}".format(vr_ticks, vl_ticks)) + rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks) + + try: + # This is a hack way to keep a poorly tuned PID from making noise at speed 0 + + # if vr_ticks is 0 and vl_ticks is 0: + # roboclaw.ForwardM1(self.address, 0) + # roboclaw.ForwardM2(self.address, 0) + # else: + # roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks) + + if vr_ticks is 0 and vl_ticks is 0: + roboclaw.ForwardM1(self.address, 0) + roboclaw.ForwardM2(self.address, 0) + self.vr_ticks = 0 + self.vl_ticks = 0 + else: + gain = 0.5 + self.vr_ticks = gain * vr_ticks + (1 - gain) * self.vr_ticks + self.vl_ticks = gain * vl_ticks + (1 - gain) * self.vl_ticks + roboclaw.SpeedM1M2(self.address, int(self.vr_ticks), int(self.vl_ticks)) + except OSError as e: + rospy.logwarn("SpeedM1M2 OSError: %d", e.errno) + rospy.logdebug(e) class Node: def __init__(self): @@ -177,13 +236,18 @@ def __init__(self): roboclaw.ResetEncoders(self.address) self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0")) - self.TICKS_PER_METER = float(rospy.get_param("~tick_per_meter", "4342.2")) + self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) + self.PUB_ODOM = rospy.get_param("~pub_odom", "true") + self.STOP_MOVEMENT = rospy.get_param("~stop_movement", "true") - self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) + self.encodm = None + if (self.PUB_ODOM == "true"): + self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) + self.movement = Movement(self.address, self.MAX_SPEED, self.BASE_WIDTH, self.TICKS_PER_METER) self.last_set_speed_time = rospy.get_rostime() - rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback) + rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.sleep(1) @@ -196,10 +260,11 @@ def __init__(self): def run(self): rospy.loginfo("Starting motor drive") - r_time = rospy.Rate(10) + r_time = rospy.Rate(30) while not rospy.is_shutdown(): - if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1: + # stop movement if robot doesn't recieve commands for 1 sec + if (self.STOP_MOVEMENT == "true" and rospy.get_rostime() - self.movement.last_set_speed_time.to_sec() > 1): rospy.loginfo("Did not get command for 1 second, stopping") try: roboclaw.ForwardM1(self.address, 0) @@ -207,6 +272,7 @@ def run(self): except OSError as e: rospy.logerr("Could not stop") rospy.logdebug(e) + self.movement.stopped = True # TODO need find solution to the OSError11 looks like sync problem with serial status1, enc1, crc1 = None, None, None @@ -228,40 +294,17 @@ def run(self): rospy.logwarn("ReadEncM2 OSError: %d", e.errno) rospy.logdebug(e) - if ('enc1' in vars()) and ('enc2' in vars()): + if ('enc1' in vars()) and ('enc2' in vars() and enc1 and enc2): rospy.logdebug(" Encoders %d %d" % (enc1, enc2)) - self.encodm.update_publish(enc1, enc2) - + if (self.encodm): + self.encodm.update_publish(enc1, enc2) self.updater.update() + self.movement.run() r_time.sleep() def cmd_vel_callback(self, twist): - self.last_set_speed_time = rospy.get_rostime() - - linear_x = twist.linear.x - if linear_x > self.MAX_SPEED: - linear_x = self.MAX_SPEED - if linear_x < -self.MAX_SPEED: - linear_x = -self.MAX_SPEED - - vr = linear_x + twist.angular.z * self.BASE_WIDTH / 2.0 # m/s - vl = linear_x - twist.angular.z * self.BASE_WIDTH / 2.0 - - vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s - vl_ticks = int(vl * self.TICKS_PER_METER) - - rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks) - - try: - # This is a hack way to keep a poorly tuned PID from making noise at speed 0 - if vr_ticks is 0 and vl_ticks is 0: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - else: - roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks) - except OSError as e: - rospy.logwarn("SpeedM1M2 OSError: %d", e.errno) - rospy.logdebug(e) + self.movement.last_set_speed_time = rospy.get_rostime() + self.movement.twist = twist # TODO: Need to make this work when more than one error is raised def check_vitals(self, stat): diff --git a/roboclaw_node/src/roboclaw_driver/roboclaw_driver.pyc b/roboclaw_node/src/roboclaw_driver/roboclaw_driver.pyc index d6a9791a6cd4e7cf91a076519c91d39e11bfbe6a..9bf3bee4c3dfe959c02c97f377ff87a6c3c40ef3 100644 GIT binary patch delta 5362 zcmb7|e@t6d6vz8$3$HDNNTvj&b9Ebx4e6#vNS%~HMutBaBTnXETajPwI$9`&b^Nj& zR2f319GD|Ge$0WJVkvttqs;v?mQ9VZQ!=A*nNf_HiF0N;G&oQV&O!*Z-LGFE3gr>(eIlJ@_#}@x-=1bTtg>vxgTjD(Lyw7{M~&&G-Q$f*K;+%=`nRn2yPS zW-*lOnMcEmJ_;fpqk1UM)3z)+1CcOo{F3lqzUg<22-QGy&aW87V*Do{AgiHpt^i6&|g>koBPb3hS6!}ypfxy{yYU%xgYyy)Ug+>ZV zE;17c$e;cbPZ2n`yD@^TY|S%u7{L}Ju_Oje#nep5+R;JiWbF{a_-qSC1^p3W<4($2 zVo4*W!@OQBT?PrIl+I#lE1_dDDB#CkPJ+6!`v5`A?P($i$wX)CKk}Rd zv&3@jDM$0di1G@QU{-2pM98nK#w>-X2dXGaRh=FkCoD~4LFQ6Qxz$R6P0cOlYhmc0 zu;d6LObum2%HHJ!&%5Tiy|rOcQQ)(d5oE_cYE{$vL};#QMs@JXSp+rqi^%&z=T`95 zzC#e#>g))iV{64LeWG3ipE{_hmq=>AcQ>fO=cMYWyP=Kni?3OGBeiCQE;TszQ`QPy z=t}su!sY`934eI>*-g|gT%t6P-Rvdw3@t|q!su7RldUg=L$#eGD9iKx1mX6?Lzmk_ zQk11*2j>Y5)F#tG^A zktiZ$sq!d!qWgIOYlwWgwS&}6T05wP?XP`+_{(+RSQkO`4pE1Uj^SZK!|0EK;gQtn zOQE|Z8XC+UbX($FN8teI6>`z`LTLJ$L$qi zZoC4c*hysc4Ya2-qW$D~M5FFh0Qr}f8EGfhG_82)y@!ibm_twQBH`k L^hV9d5C^P#TEK3a$h!yDTpT7%OI2 zX)6b9!$>id$i@k-)uc`%y*V|Oq0ZD8Q<~0HQxn4k7B)FO$Av!iq5pokGxx*&-T!(1 z&$;KE`*dAX{c%I33HW_h=#aOwXi#IDlTfQrDD*aab-BLGY1yRDwp;6~%WGV9dZ)dv zw9ZvuQ6cWi?A9tvBN=q7p*6@CbWP(p0n{}t)*OW?PK{|W?&=mTd~F(_`PZR!&?#Z$?m7P`Es z6A~Hjk?3Nafmguxgoul+x^2_wv_OEO={zl5{1r_u{v3Ka??Fs_x-yh3@$C>fQX)~? z+8`=easuL5vHQMIawdG@2X_Tx94Th{G4W@J5k;Gp>Cc0VZ%s8xXCaQa_R6v#I&8q| zy^p&NX*p7gDEa+AZY&Pd5==y}fVi49l|F!OOK#%S{- zN@@)x@5aqgd^2371>@N7)UqYvTBUEGRVtbS<(e|Jl3dpB{fgj&OJ7hM622Hb`H!VZ-KCn~~;LVsnqyE&g?%dJkF)Gh3?c5HntN z5`2CgMb==ojQ8bBdkCrQSPuBOZBTriH*#$)#IR1WGn70IJicOQGpHp|dL5Q;T-U?u z!zkH>^(di`K@q-@5u zCY!PIEm7;`q}FI`7#!{~%BQ!*VbFM$k@DM)K~8AhH=yU)?b<>$*|+b8IPQ4XcI>NS zCT9nBO2Zv8{n;rfwoH#%?%NrFnVsn|QAEccNQlil)32fMU7!)71&r;&8a!(mUYLO6 z>v-``h~f71)9w+Jk)vMZ&LW#>4Qx%Xr81N?oJ9v$sp*+la-qNn2aJ@|y+X{I4rdzm zb|*qKSAyjSu!BlwEU#j{)wk?GO6$Sqt>K<`q4n^;#&@rwbBRR94}Jvcug z=ns1vAsuFV4`W4*XS(*+@qYoS4@-acdg&3|>sv=q9ba+(;V9+<=iHfo?4Mw8z(m*H scn8{}!GUwo^L!?9{mn{<;ct%rhu=R2pCii?Z)pM`F_A#B^=*UlALgR>M*si- From 9917d9ae0fc981801bfa45675ecb668636c67966 Mon Sep 17 00:00:00 2001 From: Seho Young Date: Wed, 24 Jul 2019 21:33:25 +0000 Subject: [PATCH 2/2] parameter bug fix --- roboclaw_node/nodes/roboclaw_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roboclaw_node/nodes/roboclaw_node.py b/roboclaw_node/nodes/roboclaw_node.py index d6f19be..faf1dff 100755 --- a/roboclaw_node/nodes/roboclaw_node.py +++ b/roboclaw_node/nodes/roboclaw_node.py @@ -242,7 +242,7 @@ def __init__(self): self.STOP_MOVEMENT = rospy.get_param("~stop_movement", "true") self.encodm = None - if (self.PUB_ODOM == "true"): + if (self.PUB_ODOM): self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.movement = Movement(self.address, self.MAX_SPEED, self.BASE_WIDTH, self.TICKS_PER_METER) self.last_set_speed_time = rospy.get_rostime() @@ -264,7 +264,7 @@ def run(self): while not rospy.is_shutdown(): # stop movement if robot doesn't recieve commands for 1 sec - if (self.STOP_MOVEMENT == "true" and rospy.get_rostime() - self.movement.last_set_speed_time.to_sec() > 1): + if (self.STOP_MOVEMENT and not self.movement.stopped and rospy.get_rostime().to_sec() - self.movement.last_set_speed_time.to_sec() > 1): rospy.loginfo("Did not get command for 1 second, stopping") try: roboclaw.ForwardM1(self.address, 0)