Running a cool simulation with Morse and ROS (A Beginner’s Guide)


G’day,

Now some advance stuff after touching the surface of ROS. But it should feel good to have something up and running. Now let’s try to run a simulated robot with ROS. Sounds pretty cool, doesn’t it? We will be using the following components in our simulation.

  • Morse – 1.3
  • ROS-Indigo
  • python 2.7 and python 3.4

Basics and Setting up…

Before we start, I should say a nightmare doesn’t come even close to the ordeal this would put you through. So it’s quite important to get this right. Otherwise you can end up in a whole heap of trouble. I’m not going to explain how to set them up in detail. But I will try my best to point out where you can go wrong.

  1. Set up Morse using manual installation (Morse Manual Installation). It did not work well when I tried installing it from the Ubuntu repository.
  2. When running cmake, make sure these cmake variables are correctly set.
    • -DBUILD_ROS_SUPPORT=ON (controls the build of ROS support in MORSE)
    • -DPYMORSE_SUPPORT=ON (pymorse is a required to implement Morse with python)
    • -DCMAKE_BUILD_TYPE=Release

    and optionally,

    • -DCMAKE_INSTALL_PREFIX (Installation path for MORSE ($MORSE_ROOT).Might come in handy if you don’t have sudo)
    • -DPYTHON_EXECUTABLE (Point to python having version >= 3.2. If you want to find where python is, try which python3)
  3. To check if Morse is correctly setup run morse check. You should get the following output.
    * Checking up your environment...
    * Running on Linux. Alright.
    * Found MORSE libraries in '/usr/local/lib/python3/dist-packages/morse/blender'. Alright.
    * Trying to figure out a prefix from the script location...
    * Default scene found. The prefix seems ok. Using it.
    * Setting $MORSE_ROOT environment variable to default prefix [/usr/local/]
    * Checking version of /usr/bin/blender... Found v.2.69.0
    * Found Blender in your PATH
    (/usr/bin/blender, v.2.69.0).
    Alright, using it.
    * Checking version of Python within Blender /usr/bin/blender... Found v.3.4.3
    * Blender and Morse are using Python 3.4.3. Alright.
    
  4. Now check if rospy is correctly setup. Open up a terminal. Type python3 and enter. Now try import rospy. If you didn’t get any errors. You’re a rare gem on this planet.
  5. However, if you are like the common, like me this page will help you (Errors of rospy)

Implementing the Simulation Scene

Now assuming you got everything correct, let’s move on to the next part. Running Morse. Let’s write a script for a simulation. This simulation be located inside a classroom and will have a robot, and the robot will have a camera and a laser. Pretty cool huh!

  1. Open up the terminal, type mkdir morse_tut && cd morse_tut
  2. Fire up your favorite python IDE and create morse_tut_scene.py in the newly created directory.
  3. Now put the following code in there.
    from morse.builder import *
    
    atrv = ATRV() #the robot
    atrv.translate(x=2.5, y=3.2, z=0.0) #starting location
    
    # An odometry sensor to get odometry information
    odometry = Odometry()
    atrv.append(odometry)
    odometry.add_interface('ros', topic="/odom",frame_id="odom", child_frame_id="base_link")
    
    # Allows to control the robot with the keyboard
    keyboard = Keyboard()
    atrv.append(keyboard)
    
    #camera 10Hz, 320x240 image
    cam_frequency = 10
    camera = VideoCamera()
    camera.translate(x = 0.7, z= 0.5)
    camera.rotate(y = -0.0)
    camera.properties(cam_width=320,cam_height=240,cam_focal=6.,cam_near=0.1,cam_far=500)
    camera.frequency(cam_frequency)
    atrv.append(camera)
    camera.add_interface('ros',topic='/camera')
    
    #laser 10Hz, 180 degrees wide with a 180 points
    scan = Hokuyo()
    scan.translate(x=0.275, z=0.252)
    atrv.append(scan)
    scan.properties(Visible_arc = True,laser_range = 30.0,resolution = 1,scan_window = 180.0)
    scan.frequency(10)
    scan.create_laser_arc()
    scan.add_interface('ros', topic='/scan',frame_id="laser", child_frame_id="base_link")
    
    #motion controller
    motion = MotionXYW()
    atrv.append(motion)
    motion.add_interface('ros', topic='/cmd_vel')
    
    # Set the environment
    env = Environment('indoors-1/indoor-1')
    
  4. To know more about various components available in Morse, go to Morse Compoments
  5. Now in the terminal, cd in to morse_tut and run morse run morse_tut_scene.py
  6. If you can see the following. You should be good. Use AWSD keys to change the perspective
    Simulation
  7. However, if you weren’t that lucky, and got an error, make sure your $PYTHONPATH looks like below. It can be different from person to person though.

    /home/user/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/indigo/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python3/dist-packages:/home/user/ros/workspace/devel/lib/python2.7/dist-packages:/opt/ros/indigo/lib/python3/dist-packages
    
  8. Here is one of my posts to install ROS and MORSE without clashing with each other.

Now as we have Morse working, let’s try doing something cool with ROS. We will attempt sending out a warning if the robot is too close to an obstacle.

  1. Fire up your favorite python IDE and add the following code chunk in there. Call the file obstacle_check.py and save it in morse_tut/
    #!/usr/bin/env python
    
    import rospy
    from sensor_msgs.msg import LaserScan
    from nav_msgs.msg import Odometry
    import numpy as np
    
    # get the laser messages
    def callback_laser(msg):
        global curr_pose
        laser_raw = msg.ranges
        laser_float = [float(r) for r in laser_raw]
    
        if np.min(laser_float)<0.5:
            print "I bumped at (X:%.2f Y:%.2f). Please Reverse."%(curr_pose[0],curr_pose[1])
    
    # get the odometry messages
    def callback_odom(msg):
        global curr_pose,curr_ori
    
        pose = msg.pose.pose # has position and orientation
    
        curr_pose = [float(pose.position.x),float(pose.position.y),float(pose.position.z)]
        curr_ori = [float(pose.orientation.x),float(pose.orientation.y),float(pose.orientation.z),float(pose.orientation.w)]
    
    if __name__=='__main__':
    
        rospy.init_node("obstacle_check_node")
        rospy.Subscriber('/scan', LaserScan, callback_laser)
        rospy.Subscriber('/odom', Odometry, callback_odom)
    
        rospy.spin() # this will block untill you hit Ctrl+C
    

Running the Simulation and Obstacle check

  1. Open up a terminal and type roscore and enter
  2. Open another terminal and cd morse_tut/ then, morse run morse_tut_scene.py
  3. Open another terminal and cd morse_tut/ then, python obstacle_check.py
  4. You should see this in the obstacle_check.py terminal when you’re too close to an obstacle
    user@pc:~/morse_tut$ python obstacle_check.py 
    I bumped at (X:-2.78 Y:2.85). Please Reverse.
    I bumped at (X:-2.88 Y:2.85). Please Reverse.
    I bumped at (X:-2.95 Y:2.85). Please Reverse.
    I bumped at (X:-2.96 Y:2.85). Please Reverse.
    I bumped at (X:-2.94 Y:2.85). Please Reverse.