# Simulate an Industrial Environment¶

This notebook is additional material for the ROS Developers Live Class n.20 created and provided for free by Ephson Guakro and Ricardo Tellez of The Construct. You can distribute this notebook as long as you provide copy of this paragraph with it.

## Why this class?¶

This class is a conglomeration of all previous live classes [16, 17, 18 and 19] into a single unit.

The goal is to show how the various functionalities demonstrated in the previous live classes can be put together to accomplish a single objective.

### How to use this material¶

You will need to have a free account at the ROS Development Studio. Get the account and then follow the indications below.

You will also need to watch the Live Class. You can find it here (select next cell of this notebook and press the play button):

In [1]:
from IPython.display import YouTubeVideo

Out[1]:

## Today on ROS - LIVE class n°20, we will be learning about how to:¶

1. Learn about the ariac_package, a full Gazebo+ROS simulation
2. Create config files for proper launch of the simulation.
3. Set the robots to some initial state before work begins.
4. Write ROS code to make the robots perform an ARIAC task

We will be using the ariac_package provided by Open Robotics for the Agile Robotics Industrial Automation Competition (ARIAC) 2017. And for that, we need to set some grounds to familiarize you with the package provided. The process for today will follow the following steps:

• What the purpose of ARIAC is and what the test scenarios provided entails.
• Introduction of certain terminologies used in ARIAC.
• The work environment, what is? and where to locate what.
• The configuration parameters
• Before we proceed ...
• The launch file
• Factory operation
• Automation

A working snapshot of the work environment we are going to use.

## Step 1. About the ariac_package¶

So you don't need to download anything because we have downloaded and compiled the package for you already in the simulation workspace of the project file attached to this Live Class. Let's proceed with;

### What the purpose of ARIAC is and what the test scenarios provided entails.¶

ARIAC is robot competition centered an industrial scenario that are based around building kits made up of particular parts. The competition normally consists of 15 trials: 5 trials of each of 3 scenarios. The three (3) scenarios are;

• Baseline Kit Building: The task for this scenario is to pick specific parts and place them on a tray.
• Dropped parts: The robot will need to recover after dropping a part and complete the given Order. Recovery in this sense means either going after the dropped part or fetching a new part to complete the goal.
• In-Process Kit Change: While the robot is in the middle of assembling a kit, a new high priority order will be received that needs to be completed as fast as possible. The robot will need to decide how best to complete this new order, and then complete the previous order.
In [ ]:
YouTubeVideo('95fC6mh34jI')


### Introduction of certain terminologies used in ARIAC.¶

Certain terminologies and what they mean relative to this context.

• Order: A list of parts and their goal location on a tray.

• Part: Refers to one element of an order.

• Tray: A surface that hold parts.

• Kit: A tray and set of parts that make up an order.

### The work environment, what is? and where to locate what.¶

What is in the work enviroment provided for us?

• A 1m wide Conveyor Belt moving at fixed speed of 0.2m/s.

• Automated Guided Vehicles (AGV) with Tray. Kits are built on top of those trays.

• Trays with dimensions 0.5 x 0.7 m.

• A 6-axis Robot Arm (UR10) equipped with 'Vacuum Gripper' and mounted on a 'linear actuator - 4 meters long'. The Vacuum Gripper has a binary control and reports as to whether or not a part has been successfully gripped.

More on UR10

To launch the whole simulation we can do the following command:

In [ ]:
> cd simulation_ws
> source install/setup.bash

In [ ]:
> rosrun osrf_gear gear.py --development-mode -f /home/user/simulation_ws/src/ariac/osrf_gear/config/sample.yaml


Open the Gazebo window to see the simulation: Tools->Gazebo

However, that line is very complex so we are going to use our knowledge of ROS to integrate it inside a launch file that simplyfies the launching of the simulation.

## Step 2. Package creation and configuration files for launching the simulation with a desired configuration¶

### Package creation¶

• Navigate to simulation_ws/src folder and create a package using;
In [ ]:
> catkin_create_pkg industrial_control rospy

In [ ]:
<build_depend>osrf_gear</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>trajectory_msgs</build_depend>

• Go to the CMakeLists.txt and modify it to look like this;
In [ ]:
cmake_minimum_required(VERSION 2.8.3)
project(industrial_control)

find_package(catkin REQUIRED COMPONENTS
rospy
osrf_gear
sensor_msgs
std_srvs
trajectory_msgs
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES industrial_control
#  CATKIN_DEPENDS rospy
#  DEPENDS system_lib
)

###########
## Build ##
###########

include_directories(
${catkin_INCLUDE_DIRS} ) ## Mark executables and/or libraries for installation #install(TARGETS${PROJECT_NAME} ${PROJECT_NAME}_node # ARCHIVE DESTINATION${CATKIN_PACKAGE_LIB_DESTINATION}
#LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} #RUNTIME DESTINATION${CATKIN_PACKAGE_BIN_DESTINATION}
#)

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
config/sample_gear_conf.yaml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_industrial_control.cpp)
# if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test)  ### The configuration parameters¶ • Create a folder in industrial_control package called config. And a create YAML file inside of it with the name my_config.yaml. Copy and paste the following into the my_config.yaml file. In [ ]: # Competitor trial configuration options arm: type: ur10 sensors: break_beam: type: break_beam pose: xyz: [1.6, 2.25, 0.91] rpy: [0, 0, 'pi'] proximity_sensor: type: proximity_sensor pose: xyz: [0.95, 2.6, 0.916] rpy: [0, 0, 0] logical_camera: type: logical_camera pose: xyz: [-0.66, 0.22, 1.36] rpy: ['pi/2', 1.32, 0] logical_camera_over_agv1: type: logical_camera pose: xyz: [0.3, 3.15, 1.5] rpy: [0, 'pi/2', 0] laser_profiler: type: laser_profiler pose: xyz: [1.21, 4, 1.64] rpy: ['-pi', 'pi/2', 'pi/2']  The configuration file has three main uses: • For specifying the type and number of sensors as well as their respective position and orientation in the work envelope. Examples include the break_beam sensor, proximity_sensor etc. • For specifying what objects/ parts are spawned in the simulation and the frequency of spawn. • For specifying what type of orders are expected of this scenario Follow this link for more on how to prepare your package for use with c++ ### Before we proceed, ....¶ You may have noticed that; • The gear.py script located in install > lib > osrf_gear uses the python em module for passing the parameters specified in the configuration file to the gear.world.template. Simpy put, the 'python em' module allows for writing python syntax in none '.py' fils. For example; - line 323: if 'options' has a key called 'insert_agvs', then enter the for loop and spawn the ** AGVs **  • Also, in the gear.world file, notice the model inclusions (line 328) alongside some custom plugins (line 348) as was taught in the Live Class 18 and Live Class 19 respectively. • Take notice also of how a completely new model can be created inside of the world file either by the use of models from the models folder or by the use of sdf model creation syntax as was explained in 'Live Class 18' • Below is a snapshot of the joint controller configuration (the controller type used, tolerances and PID values etc.) used for the arm (ariac/osrf_gear/launch/ur10/arm_controller_ur10_custom.yaml) For an extended explanation of the controllers and how they are used, checkout the Live Class 17 ### The launch file¶ • Create a launch folder and create a launch file inside it called simulate.launch. Copy and paste the following code into it. In [ ]: <launch> <arg name="config_file" default="$(find industrial_control)/config/my_config.yaml"/>

<node pkg="osrf_gear" type="gear.py" name="ariac_node" output="screen"
args="--development-mode -f \$(arg config_file)" />
</launch>

• Now go into your simulation_ws folder and run catkin_make to build our package.
In [ ]:
> catkin_make --pkg industrial_control

##### Let us now bring up the simulation to see how all these parts fit together. Open a terminal and run;¶
In [ ]:
> source install/setup.bash


Now to bring up the simulation run;

In [ ]:
> roslaunch industrial_control simulate.launch


The launch will bringup an industrial environment similar to this:

• Now close the simulation and let's prepare our config file for the work at hand;

### Today's objective¶

The objective today is to fill the trays on the AGVs with parts from the the bins and conveyor belt using the robot arm. And for that we need to configure the environment to suit our purpose. Edit the my_config.yaml file to look like this;

In [ ]:
# In this scenario:
# - there is one order consisting of two types of parts;
#                         type 1 in the bins
#                         type 2 on the conveyor belt

options:
insert_models_over_bins: true
fill_demo_tray: false
belt_population_cycles: 20
model_type_aliases:
order_part1: piston_rod_part
order_part2: gear_part

belt_parts:
gear_part:
6.0:
pose:
xyz: [0.0, 0.0, 0.1]
rpy: [0, 0, 'pi/2']

orders:
order_0:
announcement_condition: time
announcement_condition_value: 0.0
parts:
part_0:
type: order_part1
pose:
xyz: [0.74, 0.05, 0]
rpy: [0, 0, 0]
part_1:
type: order_part1
pose:
xyz: [0.74, -0.03, -0.1]
rpy: [0, 0, 0]
part_2:
type: order_part2
pose:
xyz: [0.73, -0.17, -0.21]
rpy: [0, 0, 0]
part_3:
type: order_part2
pose:
xyz: [0.74, -0.14, -0.1]
rpy: [0, 0, 0]

models_over_bins:
bin7:
models:
order_part1:
xyz_start: [0.1, 0.1, 0.0]
xyz_end: [0.5, 0.5, 0.0]
rpy: [0, 0, 'pi/4']
num_models_x: 3
num_models_y: 4

bin8:
models:
order_part2:
xyz_start: [0.1, 0.1, 0.0]
xyz_end: [0.5, 0.5, 0.0]
rpy: [0, 0, 0]
num_models_x: 3
num_models_y: 3

time_limit: -1

# Competitor trial configuration options

arm:
type: ur10

sensors:
break_beam:
type: break_beam
pose:
xyz: [1.6, 2.25, 0.91]
rpy: [0, 0, 'pi']
proximity_sensor:
type: proximity_sensor
pose:
xyz: [0.95, 2.6, 0.916]
rpy: [0, 0, 0]
logical_camera:
type: logical_camera
pose:
xyz: [-0.66, 0.22, 1.36]
rpy: ['pi/2', 1.32, 0]
logical_camera_over_agv1:
type: logical_camera
pose:
xyz: [0.3, 3.15, 1.5]
rpy: [0, 'pi/2', 0]
laser_profiler:
type: laser_profiler
pose:
xyz: [1.21, 4, 1.64]
rpy: ['-pi', 'pi/2', 'pi/2']


Below is a break down of the configuration used in our setup;

• insert_models_over_bins: Whether or not to insert the models that are specified in models_over_bins
• fill_demo_tray: If true, AGV1's tray will have parts from the first order spawned
• belt_population_cycles: How many of each belt part to spawn on the belt.
• model_type_aliases: Basically for assigning alternate names to parts.
• time_limit: Maximum time allowed for this scenario (-1 means infinite)
• Under the belt_parts parameter, we get;
• name of model to be spawned
• time between consecutive spawns
• pose of the model at spawn time
• Under the models_over_bins parameter you specify;
• name of bin to spawn over
• list of models to be spawned
• type of the spawned model
• xyz_start: Origin of the first model to insert
• xyz_end: Origin of the last model to insert
• You must also specify the number of models to be spawned on the 'x' and 'y' axis ( num_models_x, num_models_y )

More on the configuration parameters

## Step 3. Set the robots to some initial state before work begins.¶

Run the code below in the terminal to see the services and topics available for controlling and reading the states of the work environment.

In [ ]:
> rosservice list
> rostopic list


To control the robot arm, run;

In [ ]:
rostopic pub -1 /ariac/arm/command trajectory_msgs/JointTrajectory "{joint_names:['elbow_joint', 'linear_arm_actuator_joint', 'shoulder_lift_joint', 'shoulder_pan_joint','wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], points:[{time_from_start:{secs: 1}, positions: [2.5, 0.0, -1.2, 3.20, 3.5, -1.51, 0.0,0.0]} ]}"


The code above uses trajectory_msgs/JointTrajectory to control the arm. And what it simply means is that, map;

- elbow_joint : 2.5
- linear_arm_actuator_joint : 0.0
- shoulder_lift_joint : -1.2
- shoulder_pan_joint : 3.20
- wrist_1_joint : 3.5
- wrist_2_joint : -1.51
- wrist_3_joint : 0.0



The extra '0.0' maps to the vacuum gripper but since that is a fixed joint, it can be ignore in the joint_names but should be included in the positions or velocities. The last position value '0.0' can however be ignored if allow_partial_joints_goal is set to True in the controller file. Visit for more on JointTrajectory messages.

## Step 4. Write ROS code to make robots perform a task¶

So far;

• We have created a package for interfacing with the enviroment.
• Created the required folder structures.
• Created the configuration file for our chosen scenario.
• Interacted with the enviroment via shell commands

Now;

• We are going to define the logic of the task we want accomplished.

### Factory operation¶

Create a scripts folder in your package. In your scripts folder, create a python file called tasker.py and fill it with the code below.

In [ ]:
#! /usr/bin/env python
import sys, math
from copy import deepcopy
sys.path.append('/home/user/simulation_ws/src/ariac/ariac_example/src/') #setting the python path
from ariac_example import ariac_example as ariac
import rospy
from osrf_gear.msg import Proximity

from osrf_gear.msg import Order
from osrf_gear.msg import VacuumGripperState
from osrf_gear.srv import AGVControl
from osrf_gear.srv import VacuumGripperControl
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from std_srvs.srv import Trigger
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

def __init__(self):
self.beam_sub = rospy.Subscriber("/ariac/break_beam", Proximity, self.beam_breaker_callback)
self.iter_num = 0
self.goal_pos = None
self.fetch_from_belt = False
self.fetch_from_bin = False
self.has_zeroed_on_belt = False
self.do_work = False
self.break_beam_state = False
self.num_part_required = 0
self.count = 0
self.x_axis_step = 0.13
[2.5, 0.0, -1.2, 3.20, 3.5, -1.51, 0.0,0.0], # ready state
[2.09, 0.25, -0.47, 3.2, 3.1, -1.51, 0.0,0.0], # pick gear_part
[2.5, 0.25, -1.0, 3.2, 3.0, -1.51, 0.0,0.0], # lift gear_part
[2.5, 2.0, -1.0, 1.5, 3.0, -1.51, 0.0,0.0], # goto AGV 1
[1.02, 2.0, -0.27, 1.5, 4.0, -1.51, 0.0,0.0], # place on agv1::tray
[2.6, 2.0, -1.0, 3.2, 3.0, -1.51, 0.0,0.0], # retract arm
[2.6, 0.38, -1.0, 3.2, 3.0, -1.51, 0.0,0.0], # go to bin for another part (and iterate)

#CONVEYOR BELT OPS
[2.7, 1.88, -1.5, 0.0, 3.5,-1.51, 0.0,0.0], # take position by conveyor belt
[1.7, 1.84, -0.8, 0.0, 3.7, -1.57, 0.0,0.0], # take position correction 1
[1.65, 1.84, -0.7, 0.0, 3.7, -1.57, 0.0,0.0], # position gripper on belt
[2.7, 1.84, -1.5, 1.5, 3.5,-1.51, 0.0,0.0], # lift off belt correction 1

]

#Beam breaker callback
def beam_breaker_callback(self, data): #sets a certain variable to initiate certain actions
self.break_beam_state = data.object_detected

def connect_callbacks(self):
comp_state_sub = rospy.Subscriber(
"/ariac/competition_state", String, self.comp_state_callback)
order_sub = rospy.Subscriber("/ariac/orders", Order, self.order_callback)
joint_state_sub = rospy.Subscriber(
"/ariac/joint_states", JointState, self.joint_state_callback)
gripper_state_sub = rospy.Subscriber(
"/ariac/gripper/state", VacuumGripperState, self.gripper_state_callback)

def send_arm_to_state(self, positions): #this override accepts multiple trajectories
msg = JointTrajectory()
msg.joint_names = self.arm_joint_names

points_dict = dict()
for idx,goal in enumerate(positions):
key = 'point_' + str(idx)
points_dict[key] = JointTrajectoryPoint()
points_dict[key].positions = goal
points_dict[key].time_from_start = rospy.Duration(float(idx + 1))

msg.points = points_dict.values()
self.joint_trajectory_publisher.publish(msg)

def main(self, _do_work=True, _fetch_from_bin=True, _fetch_from_belt=True, _num_parts=2):
self.connect_callbacks()
ariac.start_competition() #start competition

if not self.has_been_zeroed:
self.has_been_zeroed = True
rospy.sleep(1.5)
self.do_work = _do_work #set to enable arm to do work
self.fetch_from_bin = _fetch_from_bin #set to allow arm to fetch parts from the bin
self.fetch_from_belt = _fetch_from_belt #set to allow arm to fetch parts from the conveyor belt
self.num_part_required = _num_parts

def execute(self):

if self.do_work:

if self.fetch_from_bin: # fetch from BIN
for self.iter_num in range(self.num_part_required):
#rospy.logdebug("Loop at: %s", self.iter_num)

#fetch part
#prev_val = self.goal_pos[1]
#linear_actuator_joint mod ---> to position on next item
self.goal_pos[1] = self.goal_pos[1] + (self.x_axis_step * self.iter_num)
#rospy.logdebug("Current item to be picked from ---> to: %s ---> %s", prev_val, self.goal_pos[1] )

if self.iter_num % 2 == 0:
#elbow_joint modification
else:
#elbow_joint modification

rospy.logdebug("FETCHING PART ...")
rospy.sleep(1.0)
ariac.control_gripper(True) #enable gripper
rospy.sleep(0.2)

#deliver part to AGV1 tray
rospy.logdebug("DELIVERING PART ...")
rospy.sleep(3.0)
ariac.control_gripper(False) #disable gripper
rospy.sleep(0.2)

#retract and go back for next gear_part
rospy.logdebug("RETURNING TO BIN ...")
rospy.sleep(1.0)
rospy.sleep(1.0)

if self.fetch_from_belt: #fetch from CONVEYOR BELT
self.fetch_from_bin = False #preventive measure
if not self.has_zeroed_on_belt:
#take position by conyevor belt
rospy.logdebug("POSITIONING BY C.BELT ...")
rospy.sleep(1.5)
self.has_zeroed_on_belt = True

#wait for signal from the beam_breaker sensor
if self.break_beam_state:
rospy.sleep(0.3)
#enable gripper
ariac.control_gripper(True)
#begin to lower the arm
rospy.logdebug("LOWERING ...")
self.goal_pos[2] = -0.66
rospy.sleep(1.2)

if not self.current_gripper_state.attached:
rospy.logdebug("FAILED, RE-POSITIONING GRIPPER ...")
rospy.sleep(0.5)

rospy.logdebug("LIFT OFF BELT ...")
rospy.sleep(1.0)

if not self.current_gripper_state.attached:
rospy.logdebug("FAILED, RE-POSITIONING GRIPPER ...")
rospy.sleep(0.5)

rospy.logdebug("DELIVERING PART ...")
self.goal_pos[3] = 1.65 - (self.count * self.x_axis_step) #part positioning on tray
rospy.sleep(1.5)
#enable gripper
ariac.control_gripper(False)
rospy.sleep(0.2)

#retract arm
rospy.logdebug("RETURNING TO BELT ...")
rospy.sleep(1.0)
self.count += 1

if (self.count == self.num_part_required) or self.fetch_from_bin:
self.do_work = False
rospy.logdebug("WORK COMPLETED !!")
rospy.sleep(1.0)

#now cause AGV to move

if __name__ == '__main__':
rate = rospy.Rate(1)
try:
while not rospy.is_shutdown():
except rospy.exceptions.ROSInterruptException:



What is going on in the code?

• In line 4: we are setting the pythonpath to include the ariac_example package included in the ariac package as it contains some useful classes and functions we will be making use of.

• line 19: We create a class PerformTaskClass that inherits from ariac.MyComptetionClass

• line 33: A list of trajectories we will be using for the sole purposes of our objective today.

The code defines a couple of functions, most of which are self explanatory but summarized below;

• beam_breaker_callback(): Used in the topic subscription on line 22 for toggling the 'self.break_beam_state' variable when the beam breaker sensor picks up any object crossing it.
• connect_callbacks(): Basically instantiates some subscribers and sets their callbacks using functions from the 'ariac_example package'
• send_arm_to_state(): Is a function override of the same function in the base class used for sending tranjectory messages to control the arm. This function override allows for passing a list of trajectories.
• main(): The main function starts the competion, sets the arm to a READY position. The main function also allows for setting the number of parts/ runs that the robot should do and where the robot should operate.
• execute(): This is the main implementation of the robot logic. It runs for loop based on the number of required parts specified in main(). On first run, it follows this procedure(s): fetch part > (if successfull, deliver part to tray) > return to bin for the next part . On second run, it positions the arm at the conveyor belt and follows the following procedure(s): lower arm (if beam is broken) > pick the part (if failed, lift arm and try again on next beam break) > deliver part to tray > return to belt for the next part. And increment self.count variable. When all the parts have been successfully collected, it sets the arm to a stop position and triggers the agv to submit the tray.

• Note however that we are doing 'deepcopy' on lines 106, ... etc. so that any modification our copy doesn't affect the original value.

### Starting the automated factory¶

It's been a long run, but its worth every second don't you think so? Now run the codes below in different terminals to bring up the simulation and perform the task logic coded above.

Don't forget to set your python script as executable using;

In [ ]:
> chmod +x tasker.py


Now let's launch first the simulation with the proper configuration:

In [ ]:
> roslaunch industrial_control simulate.launch


Now let's launch our ROS code that will control the robots and make them start preparing orders.

In [ ]:
> rosrun industrial_control tasker.py


Sit back and watch how the world turns according to your will.....:D

• Mission completed!!