Erle-Spider: Turn 'n' Degrees R/L - Robot_Blockly Block

This tutorial explains how to create a block for Robot_Blockly that allows the spider to turn an exact number of degrees using the IMU.

Connecting to Erle-Spider

Follow this or one of the other tutorials in the same page to connect to the Erle-Brain 2. Using a WiFi hotspot is recommended but you can also use a direct Ethernet connection to access the internet.

Cloning the code and compiling it

cd ~/spider_ws/src
git clone https://github.com/erlerobot/robot_blockly
cd robot_blockly/frontend
git clone https://github.com/erlerobot/blockly
git clone https://github.com/erlerobot/ace-builds.git
cd ~/spider_ws/
catkin_make_isolated --pkg robot_blockly --install
source install_isolated/setup.bash

Running ROS nodes

The following command will run the web server and the backend.

roslaunch robot_blockly robot_blockly.launch

Make sure apache is not running or change server port in this file, spider_ws/src/robot_blockly/scripts/pywebserver.py.

Trying it out

Open http://erle-brain-2.local/ or Erle-Brains current IP in your browser.

Let's make a simple execution example. Create the following workspace or load it from a file. To do that you can use the file at the end of the page called "Blockly workspace example".

The workspace in the main page should look like this.

IMPORTANT: 'Turn L/R n degrees' blocks uses Imu values, so if the Imu talker node is not already running, 'Calibrate IMU' must be launched first. Calibration lasts 10 seconds.

Now you are ready to launch the execution.

The following video shows a demo of this example workspace.

Code explanation

To create a new spider block in Robot_Blockly we need to edit two files:

• spider_ws/src/robot_blockly/frontend/blockly/blocks/erle-spider.js

• spider_ws/src/robot_blockly/frontend/blockly/generators/python/erle-spider.js

The file inside /blocks creates the shape of the block.

Blockly.Blocks['spider_turn_right_degrees'] = {
  init: function() {
    this.appendDummyInput()
        .appendField("Turn right")
        .appendField(new Blockly.FieldTextInput("90"), "DEGREES")
        .appendField("degrees");
    this.setPreviousStatement(true);
    this.setNextStatement(true);
    this.setColour(260);
    this.setTooltip('');
    this.setHelpUrl('http://www.erlerobotics.com/');
  }
};

The file inside /generators sends the python code that will be executed in the backend.

Blockly.Python['spider_turn_right_degrees'] = function(block) {
    var degrees = block.getFieldValue('DEGREES');
	var code = ""
	code+="import rospy\n"
	code+="import subprocess\n"
	code+="import rosnode\n"
	code+="##from sensor_msgs.msg import MagneticField\n"
	code+="from sensor_msgs.msg import Imu\n"
	code+="\n"
	code+="import sys\n"
	code+="from crab_msgs.msg import apm_imu\n"
	code+="from crab_msgs.msg import BodyCommand\n"
	code+="from crab_msgs.msg import BodyState\n"
	code+="from crab_msgs.msg import GaitCommand\n"
	code+="from crab_msgs.msg import LegIKRequest\n"
	code+="from crab_msgs.msg import LegJointsState\n"
	code+="from crab_msgs.msg import LegPositionState\n"
	code+="from crab_msgs.msg import LegsJointsState\n"
	code+="\n"
	code+="from sensor_msgs.msg import Joy\n"
	code+="\n"
	code+="msg_imu = rospy.wait_for_message('/imu9250', Imu, timeout=3)\n"
	code+="\n"
	code+="quaternion = (\n"
	code+="msg_imu.orientation.w,\n"
	code+="msg_imu.orientation.x,\n"
	code+="msg_imu.orientation.y,\n"
	code+="msg_imu.orientation.z)\n"
	code+="\n"
	code+="euler = euler_from_quaternion(quaternion)\n"
	code+="initial_yaw = abs(math.degrees(euler[2]))\n"
	code+="\n"
	code+="################\n"
	code+="## INITIALIZE ##\n"
	code+="################\n"
	code+="pub = rospy.Publisher('/joy', Joy, queue_size=10)\n"
	code+="msg = Joy()\n"
	code+="msg.header.stamp = rospy.Time.now()\n"
	code+="rate = rospy.Rate(10)\n"
	code+="\n"
	code+="valueAxe = 0.0\n"
	code+="valueButton = 0\n"
	code+="for i in range (0, 20):\n"
	code+="    msg.axes.append(valueAxe)\n"
	code+="for e in range (0, 17):\n"
	code+="    msg.buttons.append(valueButton)\n"
	code+="\n"
	code+="##########################\n"
	code+="## TURN RIGHT n DEGREES ##\n"
	code+="##########################\n"
	code+="msg.axes[2] = -1\n"
	code+="bo=True\n"
	code+="previous_yaw = initial_yaw\n"
	code+="loop = 0\n"
	code+="degrees = 0\n"
	code+="target = "+degrees.toString()+"*1.05\n"
	code+="while not rospy.is_shutdown() and bo:\n"
	code+="    msg_imu = rospy.wait_for_message('/imu9250', Imu, timeout=5)\n"
	code+="    quaternion = (\n"
	code+="    msg_imu.orientation.w,\n"
	code+="    msg_imu.orientation.x,\n"
	code+="    msg_imu.orientation.y,\n"
	code+="    msg_imu.orientation.z)\n"
	code+="\n"
	code+="    euler = euler_from_quaternion(quaternion)\n"
	code+="    yaw = euler[2]\n"
	code+="    yaw = math.degrees(yaw)\n"
	code+="    yaw = abs(yaw)\n"
	code+="\n"
	code+="    degrees += abs(previous_yaw - yaw)\n"
	code+="\n"
	code+="    if (degrees >= target):\n"
	code+="        bo=False\n"
	code+="        msg.axes[2] = 0\n"
	code+="    previous_yaw = yaw\n"
	code+="    pub.publish(msg)\n"
	code+="    rate.sleep()\n"
	return code;
<br/>
};

Step by step explanation

Required math functions have been manually added at spider_ws/src/robot_blockly/scripts/robot_blockly_backend.py due to python3 compatibility issues.

This implementation should work with other quaternion-to-euler libraries, so feel free to reuse the code.

import ...

• Add imports, including required Spider and Imu messages.

msg_imu = rospy.wait_for_message('/imu9250', Imu, timeout=3)

• Read the initial Imu values. Imu node is already running.

quaternion = (
msg_imu.orientation.w,
msg_imu.orientation.x,
msg_imu.orientation.y,
msg_imu.orientation.z)
 
euler = euler_from_quaternion(quaternion)

• Create the quaternion from the values read from the Imu. Then make the transformation to get Euler angles.

initial_yaw = abs(math.degrees(euler[2]))

• Save the initial YAW value.

## INITIALIZE ## ...

• This code block initializes the Spider giving it default values.

## TURN RIGHT n DEGREES ##
#[ -1- ]
msg.axes[2] = -1
bo=True
previous_yaw = initial_yaw
loop = 0
degrees = 0
#[ -2- ]
target = "+degrees.toString()+"*1.05
 
while not rospy.is_shutdown() and bo:
    #[ -3- ]
    msg_imu = rospy.wait_for_message('/imu9250', Imu, timeout=5)
    quaternion = (
    msg_imu.orientation.w,
    msg_imu.orientation.x,
    msg_imu.orientation.y,
    msg_imu.orientation.z)
    #[ -4- ]
    euler = euler_from_quaternion(quaternion)
    yaw = euler[2]
    yaw = math.degrees(yaw)
    yaw = abs(yaw)
    #[ -5- ]
    degrees += abs(previous_yaw - yaw)
    #[ -6- ]
    if (degrees >= target):
        bo=False
        msg.axes[2] = 0
    previous_yaw = yaw
    pub.publish(msg)
    rate.sleep()

• [ 1 ] Initialize variables.

• [ 2 ] Calibration. Turn 5% more in this case.

• [ 3 ] Loop. Read new Imu values and create the Quaternion.

• [ 4 ] Loop. Get new YAW value after transforming Quaternions to Euler angles.

• [ 5 ] Loop. Calculate the change of degrees. Add the difference between the current and the previous YAW values to the degrees variable. It has to be done in this way due to a rare implementation of euler_from_quaterion function, but it will work with other implementations too.

• [ 6 ] Loop. When the target degree is reached spider rotation is stopped. If not, previous_yaw value is updated.