Blocks

Below we present the Blockly documentation of Erle-Spider:

To use these blocks with your Erle-Spider connect to its WiFi network and type the following in a browser:

10.0.0.1

Additional information about Blockly can be found at our Blocky section.



This block asks Erle-Spider to stand up (or down) and get ready for movements.

import sys
import time
from crab_msgs.msg import *
from sensor_msgs.msg import Joy

standup_time=20

################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)

valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
    msg.axes.append(valueAxe)
for e in range (0, 17):
    msg.buttons.append(valueButton)

####################
## STAND UP       ##
####################
msg.buttons[3] = 1
i=0
bo=True
standup_time=standup_time/3
while not rospy.is_shutdown() and bo:
    i=i+1
    if (i>standup_time):
      bo=False
      msg.buttons[3] = 0
    pub.publish(msg)
    rate.sleep()
time.sleep(2)



This block will move forward/backwards/left/right for Z seconds. The direction is selected from the list.

import sys
import time
from crab_msgs.msg import apm_imu
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState
from sensor_msgs.msg import Joy

################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)

valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
 msg.axes.append(valueAxe)
for e in range (0, 17):
 msg.buttons.append(valueButton)

################
##    WALK    ##
################
walking_time=float(seconds)+0.4 ##manual time calibration

start = time.time()

if "forward" is dropdown_direction:
 msg.axes[1] = 1
elif "backwards" is dropdown_direction:
 msg.axes[1] = -1
elif "left" is dropdown_direction:
 msg.axes[0] = 1
elif "right" is dropdown_direction:
 msg.axes[0] = -1

bo=True
while not rospy.is_shutdown() and bo:
 sample_time = time.time()
 if ((sample_time - start) > walking_time):
  bo=False
  msg.axes[0] = 0
  msg.axes[1] = 0
 pub.publish(msg)
 rate.sleep()



Erle-Spider will turn to its left or right for X seconds. The direction is selected from the list.

import sys
import time
from crab_msgs.msg import apm_imu
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState

from sensor_msgs.msg import Joy

################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)

valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
 msg.axes.append(valueAxe)
for e in range (0, 17):
 msg.buttons.append(valueButton)

#################
##    TURN     ##
#################
walking_time=float(seconds)+0.4 #manual time calibration

start = time.time()

if "left" is dropdown_direction:
 msg.axes[2] = 1
elif "right" is dropdown_direction:
 msg.axes[2] = -1

bo=True
while not rospy.is_shutdown() and bo:
 sample_time = time.time()
 if ((sample_time - start) > walking_time):
  bo=False
  msg.axes[2] = 0
 pub.publish(msg)
 rate.sleep()



Erle-Spider will turn to its left or right for V seconds. The direction is selected from the list.

import rospy
import subprocess
import rosnode
from sensor_msgs.msg import Imu
import sys
from crab_msgs.msg import apm_imu
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState
from sensor_msgs.msg import Joy

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

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

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

################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)

valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
    msg.axes.append(valueAxe)
for e in range (0, 17):
    msg.buttons.append(valueButton)

##########################
## TURN RIGHT n DEGREES ##
##########################

bo=True
previous_yaw = initial_yaw
loop = 0
degrees_change = 0

if "left" is dropdown_direction:
 msg.axes[2] = 1
 target = float(degrees)*0.95
elif "right" is dropdown_direction:
 msg.axes[2] = -1
 target = float(degrees)*1.05

while not rospy.is_shutdown() and bo:
    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)

    euler = euler_from_quaternion(quaternion)
    yaw = euler[2]
    yaw = math.degrees(yaw)
    yaw = abs(yaw)

    degrees_change += abs(previous_yaw - yaw)

    if (degrees_change >= target):
        bo=False
        msg.axes[2] = 0
    previous_yaw = yaw
    pub.publish(msg)
    rate.sleep()



Erle-Spider will walk the selected direction M meters. This block dependes on the SLAM block to work.

import rospy
import math
import time
from crab_msgs.msg import apm_imu
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState
from sensor_msgs.msg import Joy
from geometry_msgs.msg import PoseStamped

pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
rate = rospy.Rate(10)

valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
 msg.axes.append(valueAxe)
for e in range (0, 17):
 msg.buttons.append(valueButton)

poseStamped = rospy.wait_for_message('/slam_out_pose', PoseStamped, timeout=10)

start_x = poseStamped.pose.position.x
start_y = poseStamped.pose.position.y
start_z = poseStamped.pose.position.z

if "forward" is dropdown_direction:
 msg.axes[1] = 1
elif "backwards" is dropdown_direction:
 msg.axes[1] = -1
elif "left" is dropdown_direction:
 msg.axes[0] = 1
elif "right" is dropdown_direction:
 msg.axes[0] = -1

bo=True
while not rospy.is_shutdown() and bo:
    poseStamped = rospy.wait_for_message('/slam_out_pose', PoseStamped, timeout=3)

    current_x = poseStamped.pose.position.x
    current_y = poseStamped.pose.position.y
    current_z = poseStamped.pose.position.z

    d_x = (current_x - start_x)**2
    d_y = (current_y - start_y)**2
    d_z = (current_z - start_z)**2

    #d=srqt((x2-x1)**2+(y2-y1)**2+(z2-z1)**2)
    distance_3d = math.sqrt(d_x + d_y + d_z)

    print("\n")
    print(meters)
    print(distance_3d)

    if (distance_3d > meters):
        bo=False
        msg.axes[0] = 0
        msg.axes[1] = 0
    pub.publish(msg)
    rate.sleep()