Robotics StackExchange | Archived questions

Reading bump sensors on simulated turtlebot

This must have been dealt with many times before, but as beginner with ROS I can't find an answer I understand.

Here's my code adapted from Mark Silliman

import rospy
from kobuki_msgs.msg import BumperEvent 
from geometry_msgs.msg import Twist

def processBump(data):
            print ("Bump")
            global bump
            if (data.state == BumperEvent.PRESSED):
                bump = True
            else:
                bump = False
            rospy.loginfo("Bumper Event")
            rospy.loginfo(data.bumper)

def GoForward():
        print("Starting..")

        # initiliaze
        rospy.init_node('GoForward', anonymous=False)

        # tell user how to stop TurtleBot
        rospy.loginfo("To stop TurtleBot CTRL + C")

        # What function to call when you ctrl + c    
        rospy.on_shutdown(shutdown)
        # Create a publisher which can "talk" to TurtleBot and tell it to move
        # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2
        cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
        sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, processBump)
        #TurtleBot will stop if we don't keep telling it to move.  How often should we tell it to move? 10 HZ
        r = rospy.Rate(10);

        # Twist is a datatype for velocity
        move_cmd = Twist()
        # let's go forward at 0.2 m/s
        move_cmd.linear.x = 0.2
        # let's turn at 0 radians/s
        move_cmd.angular.z = 0.0
        # as long as you haven't ctrl + c keeping doing...
        while not rospy.is_shutdown():
            # publish the velocity
            cmd_vel.publish(move_cmd)
            # wait for 0.1 seconds (10 HZ) and publish again
            r.sleep()




def shutdown(self):
        # stop turtlebot
        rospy.loginfo("Stop TurtleBot")
        # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
        cmd_vel.publish(Twist())
        # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
        rospy.sleep(1)




if __name__ == '__main__':
    try:
        GoForward()
    except rospy.ServiceException as exc:
      rospy.loginfo("GoForward node terminated.")
      print("Service did not process request: " + str(exc))

The issue is that processBump() never gets called, even though the turtlebot is moving and then clearly runs into to something.

I guess I'm not understanding the Subscriber functionality, but isn't like a listener i.e in this case, when a BumperEvent occurs the specified function, processBump() is called?

Excuse my newbieness and any clues greatly appreciated.

Cheers,

Rob

Similar to this topic: Help on my basic turtlbot node http://answers.ros.org/question/240925/help-on-my-basic-turtlbot-node/

Asked by RobB on 2016-10-01 07:29:37 UTC

Comments

Answers

Hello Rob, the callback function is working, the problem is what do you do when the bump occurs, it is only outputting informations.

Asked by zhibo on 2016-10-03 10:59:47 UTC

Comments

Thanks for answering. Yes, it's true, the code only outputs a trace. The problem is I don't see the trace when I run the program (and the turtlebot collides with something), so I have to conclude that the processBump() function is not being called.

Asked by RobB on 2016-10-04 00:32:33 UTC

i tried your code, and when i push the bumper, I can see the 'bump' message, maybe your bumper is not working, try to rostopic echo the bumper to see if it is working or not.

Asked by zhibo on 2016-10-04 03:07:50 UTC

what launch file are you using? When you say "the bumper is not working" what does that mean on a simulated turtlebot?

Asked by RobB on 2016-10-04 03:58:49 UTC

I tested on a real one. I just tested in the simulation, it is working as well with turtlebot_gazebo turtlebot_world.launch

Asked by zhibo on 2016-10-04 04:38:36 UTC

OK I'll try again. Thanx

Asked by RobB on 2016-10-04 04:59:12 UTC

Still doesn't work. So I have three terminals open: 1) roscore 2) roslaunch turtlebot_gazebo turtlebot_world.launch -- which brings up gazebo 3) nav to my script directory and run the program . python goforward2.py

Asked by RobB on 2016-10-04 05:06:23 UTC

The bot moves but when it runs into one of the objects in the world, I do not see the trace. How else can I check what the sensor is detecting? Rviz is giving me something (pointCloud?) but I'm having difficulty interpreting it.

Asked by RobB on 2016-10-04 05:08:22 UTC

Any ideas zhibo? I'm really stuck with this.

Asked by RobB on 2016-10-05 06:55:05 UTC

obviously if you put a sphere in front of turtlebot if won't work, i tested with a cube. Another to see if the bumper is working or not, do the same 3 terminals you did, in the 4th:

rostopic pub -r 1 /mobile_base/events/bumper kobuki_msgs/BumperEvent "bumper: 1"

it will trigger the bump at 1hz.

Asked by zhibo on 2016-10-05 07:13:53 UTC

and then in your 3rd terminal if you see stuffs, it means your callback function is working

Asked by zhibo on 2016-10-05 07:15:38 UTC

Thanks. Yes, the bot is hitting a flat surface as far as I can tell. I'll try your suggestion.

Asked by RobB on 2016-10-05 07:49:56 UTC

Hey zhibo ,what version of ROS are you using? Mine is kinetic.

Asked by RobB on 2016-10-06 06:07:13 UTC

i'm using indigo

Asked by zhibo on 2016-10-06 07:00:00 UTC

I think kinetic is the cause of many of my problems. What a pain! I installed xenial and then you are pretty much forced to install kinetic unless you want to build the ROS packages from source. Lots of packages are missing in kinetic too.

Asked by RobB on 2016-10-06 07:06:35 UTC