Pkg_task5

In this package there are two ros node ur5_1.py and ur5_2.py in script folder.

ur5_1.py

class Ur5Moveit

This is the class to to control ur5_1 arm.:

Below are the member function of class Ur5Moveit

__ init __(self,arg_robot_name)

docstring

The constructor for Ur5Moveit class.

Parameters:
        arg_robot_name: Arm which we want to operate.

code

        self._robot_ns = '/' + arg_robot_name
        self._planning_group = "manipulator"
        self._commander = moveit_commander.roscpp_initialize(sys.argv)
        self._robot = moveit_commander.RobotCommander(\
        robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns)
        self._scene = moveit_commander.PlanningSceneInterface\
        (ns=self._robot_ns)
        self._group = moveit_commander.MoveGroupCommander(self._planning_group,robot_description=self._robot_ns + "robot_description", ns=self._robot_ns)
        self._display_trajectory_publisher = rospy.Publisher(\
        self._robot_ns + '/move_group/display_planned_path',\
        moveit_msgs.msg.DisplayTrajectory, queue_size=1)
        self._exectute_trajectory_client = actionlib.SimpleActionClient(\
        self._robot_ns + '/execute_trajectory',\
        moveit_msgs.msg.ExecuteTrajectoryAction)
        self._exectute_trajectory_client.wait_for_server()
        self._planning_frame = self._group.get_planning_frame()
        self._eef_link = self._group.get_end_effector_link()
        self._group_names = self._robot.get_group_names()
        self._box_name = ''
        self.box_name = ""
        self.msg = ""
        self.msg1 = ""
        self.packagen00 = "packagen00"
        self.packagen01 = "packagen01"
        self.packagen02 = "packagen02"
        self.packagen10 = "packagen10"
        self.packagen11 = "packagen11"
        self.packagen12 = "packagen12"
        self.packagen20 = "packagen20"
        self.packagen21 = "packagen21"
        self.packagen22 = "packagen22"
        self.packagen30 = "packagen30"
        self.packagen31 = "packagen31"
        self.packagen32 = "packagen32"
        self.camera_1 = "camera_1"
        # Attribute to store computed trajectory by the planner
        self._computed_plan = ''
        # Current State of the Robot is needed to add box to planning scene
        self._curr_state = self._robot.get_current_state()
        #group.set_start_state(self._curr_state)
        rospy.loginfo(
                '\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
        rospy.loginfo(
                '\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
        rospy.loginfo(
                '\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m')
        r_p = rospkg.RosPack()
        self._pkg_path = r_p.get_path('pkg_task5')
        self._file_path_packagen00 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen00/'
        self._file_path_packagen01 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen01/'
        self._file_path_packagen02 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen02/'
        self._file_path_packagen10 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen10/'
        self._file_path_packagen11 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen11/'
        self._file_path_packagen12 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen12/'
        self._file_path_packagen20 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen20/'
        self._file_path_packagen21 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen21/'
        self._file_path_packagen22 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen22/'
        self._file_path_packagen30 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen30/'
        self._file_path_packagen31 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen31/'
        self._file_path_packagen32 = self._pkg_path\
        + '/config/saved_trajectories_ur5_1/packagen32/'
        self.team_id = "VB_0412"
        self.unique_id = "VsMyPsSr"
        self._config_mqtt_sub_cb_ros_topic = "/eyrc/vb/VsMyPsSr/spreadsheet/ordersdispatch"
        self._handle_ros_pub = rospy.Publisher(self._config_mqtt_sub_cb_ros_topic,msg_spreadsheet_1, queue_size=10)
        rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m')

moveit_hard_play_planned_path_from_file(self,arg_file_path, arg_file_name, arg_max_attempts)

docstring

Function to play the saved trajectories in certain 
attempts in case of failure.

Parameters:
        arg_file_path: Path of yaml file
        arg_file_name: Name of yaml file
        arg_max_attempts: No of attempts to try in case of failure.

Return:
       "True" after succesfully running the saved trajectory.

code

file_path = arg_file_path + arg_file_name
with open(file_path, 'r') as file_open:
    loaded_plan = yaml.load(file_open)
ret = self._group.execute(loaded_plan)
return ret

set_joint_angles(self, arg_list_joint_angles):

docstring

Function to execute the input joint values.

Parameters:
        arg_list_joint_values: Joint values of the ur5_1 arm

code

list_joint_values = self._group.get_current_joint_values()
print list_joint_values
self._group.set_joint_value_target(arg_list_joint_angles)
self._computed_plan = self._group.plan()
flag_plan = self._group.go(wait=True)
list_joint_values = self._group.get_current_joint_values()
pose_values = self._group.get_current_pose().pose
print pose_values
if flag_plan == True:
    pass
    # rospy.loginfo(
    #     '\033[94m' + ">>> set_joint_angles() Success" + '\033[0m')
else:
    pass
    # rospy.logerr(
    #     '\033[94m' + ">>> set_joint_angles() Failed." + '\033[0m')
return flag_plan

hard_set_joint_angles(self, arg_list_joint_angles, arg_max_attempts):

docstring

Function to execute the input joint values in certain attempts in 
case of failure.

Parameters:
        arg_list_joint_values: Joint values of the ur5_1 arm
        arg_max_attempts: No of attempts to be executed in case of failure

code

number_attempts = 0
flag_success = False
while (number_attempts <= arg_max_attempts) and  (flag_success is False):
    number_attempts += 1
    flag_success = self.set_joint_angles(arg_list_joint_angles)
    rospy.logwarn("attempts: {}".format(number_attempts))

go_to_pose(self, arg_pose):

docstring

Function to make the arm go to a predefined pose.

Parameters:
        arg_pose: Target goal pose.

code

pose_values = self._group.get_current_pose().pose
print pose_values
rospy.loginfo('\033[94m' + ">>> Current Pose:" + '\033[0m')
rospy.loginfo(pose_values)
self._group.set_pose_target(arg_pose)
flag_plan = self._group.go(wait=True)  # wait=False for Async Move
pose_values = self._group.get_current_pose().pose
rospy.loginfo('\033[94m'+ ">>> Final Pose:"+ '\033[0m')
rospy.loginfo(pose_values)
list_joint_values = self._group.get_current_joint_values()
rospy.loginfo('\033[94m'+ ">>> Final Joint Values:"+ '\033[0m')
rospy.loginfo(list_joint_values)
if flag_plan == True:
    pass
# rospy.loginfo(
    #'\033[94m' + ">>> set_joint_angles() Success" + '\033[0m')
else:
    pass
    # rospy.logerr(
    #'\033[94m' + ">>> set_joint_angles() Failed." + '\033[0m')
return flag_plan

hard_go_to_pose(self, arg_pose, arg_max_attempts):

docstring

Function to make the arm go to a predefined pose in certain attempts in 
case of failure.

Parameters:
        arg_pose: Target goal pose
        arg_max_attempts: No of attempts to execute in case of failure

code

number_attempts = 0
flag_success = False
while (number_attempts <= arg_max_attempts) and  (flag_success is False):
    number_attempts += 1
    flag_success = self.go_to_pose(arg_pose)
    rospy.logwarn("attempts: {}".format(number_attempts))

ee_cartesian_translation(self, trans_x, trans_y, trans_z):

docstring

Function to execute Cartesian path.

Parameters:
        trans_x: X-coordinate
        trans_y: Y-coordinate
        trans_z: Z-coordinate

code

# 1. Create a empty list to hold waypoints
waypoints = []
# 2. Add Current Pose to the list of waypoints
waypoints.append(self._group.get_current_pose().pose)
# 3. Create a New waypoint
wpose = geometry_msgs.msg.Pose()
wpose.position.x = waypoints[0].position.x + (trans_x)
wpose.position.y = waypoints[0].position.y + (trans_y)
wpose.position.z = waypoints[0].position.z + (trans_z)
# This to keep EE parallel to Ground Plane
wpose.orientation.x = 0
wpose.orientation.y = 0
wpose.orientation.z = -1
wpose.orientation.w = 0
# 4. Add the new waypoint to the list of waypoints
waypoints.append(copy.deepcopy(wpose))
# 5. Compute Cartesian Path connecting the waypoints in the list of waypoints
(plan, fraction) = self._group.compute_cartesian_path(
    waypoints,# waypoints to follow
    0.01,     # Step Size, distance between two adjacent computed waypoints will be 1 cm
    0.0)      # Jump Threshold
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
if num_pts >= 3:
    del plan.joint_trajectory.points[0]
    del plan.joint_trajectory.points[1]
# 6. Make the arm follow the Computed Cartesian Path
self._group.execute(plan)

wait_for_state_update(self, b_name, box_is_known=False,box_is_attached=False, timeout=4):

docstring

Function to check for state update.

Parameters:
        b_name: Box Name
        box_is_known: "True" if box is know otherwise "False"
        box_is_attached: "True" if box is attached otherwise "False"
        timeout: Time for state update

Return:
       "True" if we are in expected state or "False" if we timed out

code

# 1. Create a empty list to hold waypoints
waypoints = []
# 2. Add Current Pose to the list of waypoints
waypoints.append(self._group.get_current_pose().pose)
# 3. Create a New waypoint
wpose = geometry_msgs.msg.Pose()
wpose.position.x = waypoints[0].position.x + (trans_x)
wpose.position.y = waypoints[0].position.y + (trans_y)
wpose.position.z = waypoints[0].position.z + (trans_z)
# This to keep EE parallel to Ground Plane
wpose.orientation.x = 0
wpose.orientation.y = 0
wpose.orientation.z = -1
wpose.orientation.w = 0
# 4. Add the new waypoint to the list of waypoints
waypoints.append(copy.deepcopy(wpose))
# 5. Compute Cartesian Path connecting the waypoints in the list of waypoints
(plan, fraction) = self._group.compute_cartesian_path(
    waypoints,# waypoints to follow
    0.01,     # Step Size, distance between two adjacent computed waypoints will be 1 cm
    0.0)      # Jump Threshold
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
if num_pts >= 3:
    del plan.joint_trajectory.points[0]
    del plan.joint_trajectory.points[1]
# 6. Make the arm follow the Computed Cartesian Path
self._group.execute(plan)

bin1_pose(self, bin_x, bin_y, bin_z):

docstring

Function to give the coordinates of the packages and their respective bin.

Parameters:
        bin_x: X-coordinate
        bin_y: Y-coordinate
        bin_z: Z-coordinate

code

ur5_2_home_pose = geometry_msgs.msg.Pose()
ur5_2_home_pose.position.x = bin_x
ur5_2_home_pose.position.y = bin_y
ur5_2_home_pose.position.z = bin_z
ur5_2_home_pose.orientation.x = 0
ur5_2_home_pose.orientation.y = 0
ur5_2_home_pose.orientation.z = -1
ur5_2_home_pose.orientation.w = 0
self.hard_go_to_pose(ur5_2_home_pose, 5)

bin2_pose(self, bin_x, bin_y, bin_z):

docstring

Function to give the coordinates of the packages and their respective bin.

Parameters:
        bin_x: X-coordinate
        bin_y: Y-coordinate
        bin_z: Z-coordinate

code

box_length = 0.15               # Length of the Package
vacuum_gripper_width = 0.12    # Vacuum Gripper Width
ur5_2_home_pose = geometry_msgs.msg.Pose()
ur5_2_home_pose.position.x = bin_x
ur5_2_home_pose.position.y = bin_y
ur5_2_home_pose.position.z = bin_z + vacuum_gripper_width + (box_length/2)
ur5_2_home_pose.orientation.x = -0.5
ur5_2_home_pose.orientation.y = -0.5
ur5_2_home_pose.orientation.z = 0.5
ur5_2_home_pose.orientation.w = 0.5
self.hard_go_to_pose(ur5_2_home_pose, 5)

packagen00_box(self, data):

docstring

Function to pick the package placed at R0C0 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen00,\
'box_pos_1.yaml', 5)
self.service(True)
self.attach_box(self.packagen00)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen00,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen00)
self.remove_box(self.packagen00)

packagen01_box(self, data):

docstring

Function to pick the package placed at R0C1 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen01,\
'box_pos_8.yaml', 5)
self.service(True)
self.attach_box(self.packagen01)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen01,\
'conv_pos_9.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen01)
self.remove_box(self.packagen01)

packagen02_box(self, data):

docstring

Function to pick the package placed at R0C2 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen02,\
'box_pos_1.yaml', 5)
self.service(True)
self.attach_box(self.packagen02)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen02,\
'conv_pos_2.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen02)
self.remove_box(self.packagen02)

packagen10_box(self, data):

docstring

Function to pick the package placed at R1C0 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen10,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen10)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen10,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen10)
self.remove_box(self.packagen10)

packagen11_box(self, data):

docstring

Function to pick the package placed at R1C1 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen11,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen11)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen11,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen11)
self.remove_box(self.packagen11)

packagen12_box(self, data):

docstring

Function to pick the package placed at R1C2 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen12,\
'box_pos_8.yaml', 5)
self.service(True)
self.attach_box(self.packagen12)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen12,\
'conv_pos_8.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen12)
self.remove_box(self.packagen12)

packagen20_box(self, data):

docstring

Function to pick the package placed at R2C0 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen20,\'box_pos_2.yaml', 5)
self.service(True)
self.attach_box(self.packagen20)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen20,\
'conv_pos_2.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen20)
self.remove_box(self.packagen20)

packagen21_box(self, data):

docstring

Function to pick the package placed at R2C1 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen21,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen21)
self.ee_cartesian_translation(0, 0.1, 0)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen21,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen21)
self.remove_box(self.packagen21)

packagen22_box(self, data):

docstring

Function to pick the package placed at R2C2 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen22,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen22)
self.ee_cartesian_translation(0, 0.1, 0)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen22,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen22)
self.remove_box(self.packagen22)

packagen30_box(self, data):

docstring

Function to pick the package placed at R3C0 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen30,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen30)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen30,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen30)
self.remove_box(self.packagen30)

packagen32_box(self, data):

docstring

Function to pick the package placed at R3C2 position.

Parameters:
        data: Relevant order data linked to this package.

code

order_list = data
rospy.logwarn("1. Playing AllZeros to Pose#1 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen32,\
'box_pos.yaml', 5)
self.service(True)
self.attach_box(self.packagen32)
rospy.logwarn("2. Playing Pose#1 to Pose#2 Trajectory File")
self.moveit_hard_play_planned_path_from_file(self._file_path_packagen32,\
'conv_pos.yaml', 5)
self.service(False)
self.dispatch_order(order_list)
self.detach_box(self.packagen32)
self.remove_box(self.packagen32)

add_box(self, b_name, x, y, z, timeout=4):

docstring

Function to add box in rviz.

Parameters:
        b_name: Box name we want to add
        x: X-coordinate of box
        y: Y-coordinate of box
        z: Z-coordinate of box
        timeout: Time for state update

code

box_name = b_name
scene = self._scene
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "world"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = x_a
box_pose.pose.position.y = y_a
box_pose.pose.position.z = z_a
scene.add_box(box_name, box_pose, size=(0.15, 0.15, 0.15))
return self.wait_for_state_update(b_name, box_is_known=True,\
timeout=timeout)

add_box1(self, b_name, x, y, z, timeout=4):

docstring

Function to add 2D camera in rviz.

Parameters:
        b_name: Name we want to add
        x: X-coordinate of camera
        y: Y-coordinate of camera
        z: Z-coordinate of camera
        timeout: Time for state update

code

box_name = b_name
scene = self._scene
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "world"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = x_a
box_pose.pose.position.y = y_a
box_pose.pose.position.z = z_a
scene.add_box(box_name, box_pose, size=(0.08, 0.10, 0.10))
return self.wait_for_state_update(b_name, box_is_known=True,\
timeout=timeout)

attach_box(self, b_name, timeout=4):

docstring

Function to attach box from robotic arm in rviz.

Parameters:
        b_name: Name of the box
        timeout: Time for state update

code

box_name = b_name
scene = self._scene
eef_link = self._eef_link
scene.remove_attached_object(eef_link, name=box_name)
self.service(False)
return self.wait_for_state_update(b_name, box_is_known=True,\
box_is_attached=False, timeout=timeout)

detach_box(self, b_name, timeout=4):

docstring

Function to detach box from robotic arm in rviz.

Parameters:
        b_name: Name of the box
        timeout: Time for state update

code

box_name = b_name
scene = self._scene
eef_link = self._eef_link
scene.remove_attached_object(eef_link, name=box_name)
self.service(False)
return self.wait_for_state_update(b_name, box_is_known=True,\
box_is_attached=False, timeout=timeout)

service(self, ur5_msg):

docstring

Function to to operate the Vacuum Gripper(using service).

Parameters:
        ur5_msg: "True" or "False" depending upon whether we want to
         switch on or off the Vacuum Gripper.

code

self.msg = ur5_msg
rospy.wait_for_service('/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1')
sos_service = rospy.ServiceProxy('\/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1', vacuumGripper)
sos = vacuumGripperRequest(self.msg)
result1 = sos_service(sos)
print result1

remove_box(self, b_name, timeout=4):

docstring

Function to remove box from robotic arm in rviz.

Parameters:
        b_name: Name of the box
        timeout: Time for state update

code

box_name = b_name
scene = self._scene
scene.remove_world_object(box_name)
return self.wait_for_state_update(b_name, box_is_attached=False,\
box_is_known=False, timeout=timeout)

joint_angles(self, a_angle, b_angle, c_angle, d_angle, e_angle, f_angle):

docstring

Function to convert the input angle(in degrees) to radian using "math" library.

Parameters:
        a_angle, b_angle, c_angle, d_angle, e_angle, f_angle: Six joint angle of ur5_2 arm.

code

lst_joint_angles_1 = [math.radians(a_angle*57.29577951),\math.radians(b_angle*57.29577951), math.radians(c_angle*57.29577951),\math.radians(d_angle*57.29577951), math.radians(e_angle*57.29577951),\math.radians(f_angle*57.29577951)]

self.hard_set_joint_angles(lst_joint_angles_1, 5)

conveyor(self, conveyor_msg):

docstring

Function to to operate Coneyor Belt(using ros service).

Parameters:
        conveyor_msg: Power on which we want to operate the Conveyor Belt.

code

self.msg1 = conveyor_msg
rospy.wait_for_service('/eyrc/vb/conveyor/set_power')
sos_service = rospy.ServiceProxy('/eyrc/vb/conveyor/set_power',\
conveyorBeltPowerMsg)
sos = conveyorBeltPowerMsgRequest(self.msg1)
result1 = sos_service(sos)
print result1

get_time_str(self):

docstring

Function to get the current date time parameter in the prescribed format.

code

timestamp = int(time.time())
value = datetime.datetime.fromtimestamp(timestamp)
str_time = value.strftime('%Y-%m-%d %H:%M:%S')
return str_time

mqtt_orders_1(self, data):

    Callback function of '/eyrc/vb/VsMyPsSr/mqtt/orders' this ros topic.

    Parameters:
            data: Message that is published on this topic.

mqtt_orders_2(self, data):

docstring

Callback function of "/eyrc/vb/logical_camera_2" this ros topic.

Parameters:
        data: Message that is published on this topic.

code

global LST
global order # List storing order id's and it's respective information.
order_list = data.message
print order_list
goal_message = (order_list.split(", "))
order_id1 = (goal_message[2])[13:len(goal_message[2])-1]
order.append((order_id1, order_list))
LST.append(order_id1) # List storing order id's
print LST

dispatch_order(self, order_id):

docstring

Function to sort the dispatch orders on the basis of their oder id's.

Parameters:
        order_id: Order ID of order.

code

global order
for j in order:
    if j[0] == order_id:
        print j
        self.msg_order_1(j[1])
        order.remove(j)

msg_order_1(self, payload):

docstring

Function to publish on "/eyrc/vb/VsMyPsSr/spreadsheet/ordersdispatch" 
this ros topic.

Parameters:
        data: Message that is published on this topic.

code

global LST
global order # List storing order id's and it's respective information.
order_list = data.message
print order_list
goal_message = (order_list.split(", "))
order_id1 = (goal_message[2])[13:len(goal_message[2])-1]
order.append((order_id1, order_list))
LST.append(order_id1) # List storing order id's
print LST

__ del __(self):

docstring

Destructor function to delete the class object.

code

moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class Ur5Moveit Deleted." + '\033[0m')

node ur5_2.py

class CartesianPath:

This is the class to to control ur5_2 arm.

Below are the member function of this class

__ init __(self, arg_robot_name):

    The constructor for CartesianPath class.

set_joint_angles(self, arg_list_joint_angles):

docstring

Function to execute the input joint values.

Parameters:
        arg_list_joint_values: Joint values of the ur5_1 arm

Return:
     flag_plan: "True" if the goal is reached or "False" if the goal is failed.

code

list_joint_values = self._group.get_current_joint_values()
print list_joint_values
self._group.set_joint_value_target(arg_list_joint_angles)
self._computed_plan = self._group.plan()
flag_plan = self._group.go(wait=True)
list_joint_values = self._group.get_current_joint_values()
pose_values = self._group.get_current_pose().pose
print pose_values
if flag_plan == True:
    pass
    # rospy.loginfo(
    #     '\033[94m' + ">>> set_joint_angles() Success" + '\033[0m')
else:
    pass
    # rospy.logerr(
    #     '\033[94m' + ">>> set_joint_angles() Failed." + '\033[0m')
return flag_plan

hard_set_joint_angles(self, arg_list_joint_angles, arg_max_attempts):

docstring

Function to execute the input joint values in certain attempts in case 
of failure.

Parameters:
        arg_list_joint_values: Joint values of the ur5_1 arm
        arg_max_attempts: No of attempts to be executed in case of failure

code

number_attempts = 0
flag_success = False
while ((number_attempts <= arg_max_attempts) and  (flag_success is False):
    number_attempts += 1
    flag_success = self.set_joint_angles(arg_list_joint_angles)
    rospy.logwarn("attempts: {}".format(number_attempts))

conveyor(self, conveyor_msg):

docstring

Function to to operate Coneyor Belt(using ros service).

Parameters:
        conveyor_msg: Power on which we want to operate the Conveyor Belt.

code

self.msg1 = conveyor_msg
rospy.wait_for_service('/eyrc/vb/conveyor/set_power')
sos_service = (rospy.ServiceProxy('/eyrc/vb/conveyor/set_power',\

conveyorBeltPowerMsg)) sos = conveyorBeltPowerMsgRequest(self.msg1) result1 = sos_service(sos) print result1

get_qr_data(self, arg_image):

docstring

Function to decode the image for various packages.

Parameters:
        arg_image: Raw image to be decoded.

code

        global LIST_A
        qr_result = decode(arg_image)
        print len(qr_result)
        box_color = []
        for i in qr_result:
        x_a, y_a, h_a, w_a = i.rect
        if (x_a > 100 and x_a < 200) and (y_a > 250 and y_a < 400):
                box_color.append(('packagen00', i.data))
        if (x_a > 250 and x_a < 400) and (y_a > 250 and y_a < 400):
                box_color.append(('packagen01', i.data))
        if (x_a > 400 and x_a < 600) and (y_a > 250 and y_a < 400):
                box_color.append(('packagen02', i.data))
        if (x_a > 100 and x_a < 200) and (y_a > 400 and y_a < 550):
                box_color.append(('packagen10', i.data))
        if (x_a > 250 and x_a < 400) and (y_a > 400 and y_a < 550):
                box_color.append(('packagen11', i.data))
        if (x_a > 400 and x_a < 550) and (y_a > 400 and y_a < 550):
                box_color.append(('packagen12', i.data))
        if (x_a > 100 and x_a < 200) and (y_a > 550 and y_a < 670):
                box_color.append(('packagen20', i.data))
        if (x_a > 250 and x_a < 400) and (y_a > 550 and y_a < 670):
                box_color.append(('packagen21', i.data))
        if (x_a > 400 and x_a < 550) and (y_a > 550 and y_a < 670):
                box_color.append(('packagen22', i.data))
        if (x_a > 100 and x_a < 200) and (y_a > 670 and y_a < 850):
                box_color.append(('packagen30', i.data))
        if (x_a > 250 and x_a < 400) and (y_a > 670 and y_a < 850):
                box_color.append(('packagen31', i.data))
        if (x_a > 400 and x_a < 550) and (y_a > 670 and y_a < 850):
                box_color.append(('packagen32', i.data))
        if len(qr_result) > 0:
        SUB_ONCE.unregister()
        LIST_A = box_color
        print(w_a, h_a)
        else:
        return 'NA'

camera_1_callback(self, data):

docstring

Callback function of 2D Camera topic.

Parameters:
        arg_image: Raw image to be decoded.
        data: Message publishing on this topic

code

try:
    cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")

except CvBridgeError as err_or:
    rospy.logerr(err_or)
image = cv_image
retval, image = cv2.threshold(image, 15, 200, cv2.THRESH_BINARY)
# Resize a 720x1280 image to 360x640 to fit it on the screen
resized_image = cv2.resize(image, (720/2, 1280/2))

cv2.imshow("/eyrc/vb/camera_1/image_raw", resized_image)

rospy.loginfo(self.get_qr_data(image))

cv2.waitKey(3)

pkg1(self, trans_x, trans_y, trans_z):

docstring

Function to pick the red box and place it in the red bin.

Parameters:
        trans_x: X-coordinate of red bin.
        trans_y: Y-coordinate of red bin.
        trans_z: Z-coordinate of red bin.

code

global ORDER_LIST
shipped_data = ORDER_LIST
self.bin1_pose(trans_x, trans_y, trans_z)
self.service1(True)
rospy.sleep(0.2)
self.bin1_pose(trans_x, trans_y, trans_z+0.03)
self.ee_cartesian_translation(0, 0, 0.03)
self.conveyor(100)
self.bin1_pose(0, 0.8, trans_z+0.05)
self.service1(False)
ship_time = round(rospy.Time.now().to_sec())
self.msg_order_1(shipped_data, ship_time)
thread_ur5 = threading.Thread(target=self.bin1_pose, args=[-0.8, 0, trans_z])
thread_ur5.start()

pkg2(self, trans_x, trans_y, trans_z):

docstring

Function to pick the yellow box and place it in the yellow bin.

Parameters:
        trans_x: X-coordinate of yellow bin.
        trans_y: Y-coordinate of yellow bin.
        trans_z: Z-coordinate of yellow bin.

code

global ORDER_LIST
shipped_data = ORDER_LIST
self.bin1_pose(trans_x, trans_y, trans_z)
self.service1(True)
rospy.sleep(0.1)
self.bin1_pose(trans_x, trans_y, trans_z+0.03)
self.conveyor(100)
self.bin1_pose(0.8, 0, trans_z+0.04)
self.service1(False)
ship_time = round(rospy.Time.now().to_sec())
self.msg_order_1(shipped_data, ship_time)
thread_ur5 = threading.Thread(target=self.bin1_pose, args=[-0.8, 0, trans_z])
thread_ur5.start()

pkg3(self, trans_x, trans_y, trans_z):

docstring

Function to pick the green box and place it in the green bin.

Parameters:
        trans_x: X-coordinate of green bin.
        trans_y: Y-coordinate of green bin.
        trans_z: Z-coordinate of green bin.

code

global ORDER_LIST
shipped_data = ORDER_LIST
self.bin1_pose(trans_x, trans_y, trans_z)
self.service1(True)
rospy.sleep(0.1)
self.ee_cartesian_translation(0, 0, 0.04)
self.conveyor(100)
self.bin1_pose(0, -0.8, trans_z+0.0345)
self.service1(False)
ship_time = round(rospy.Time.now().to_sec())
self.msg_order_1(shipped_data, ship_time)
thread_ur5 = threading.Thread(target=self.bin1_pose, args=[-0.8, 0, trans_z])
thread_ur5.start()

ee_cartesian_translation(self, trans_x, trans_y, trans_z):

docstring

Function to execute Cartesian path.

Parameters:
        trans_x: X-coordinate
        trans_y: Y-coordinate
        trans_z: Z-coordinate

code

# 1. Create a empty list to hold waypoints
waypoints = []
# 2. Add Current Pose to the list of waypoints
waypoints.append(self._group.get_current_pose().pose)
# 3. Create a New waypoint
wpose = geometry_msgs.msg.Pose()
wpose.position.x = waypoints[0].position.x + (trans_x)
wpose.position.y = waypoints[0].position.y + (trans_y)
wpose.position.z = waypoints[0].position.z + (trans_z)
# This to keep EE parallel to Ground Plane
wpose.orientation.x = -0.5
wpose.orientation.y = -0.5
wpose.orientation.z = 0.5
wpose.orientation.w = 0.5
# 4. Add the new waypoint to the list of waypoints
waypoints.append(copy.deepcopy(wpose))
# 5. Compute Cartesian Path connecting the waypoints in the list of waypoints
(plan, fraction) = self._group.compute_cartesian_path(
    waypoints,# waypoints to follow
    0.01,     # Step Size, distance between two adjacent computed waypoints will be 1 cm
    0.0)      # Jump Threshold
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
if num_pts >= 3:
    del plan.joint_trajectory.points[0]
    del plan.joint_trajectory.points[1]
# 6. Make the arm follow the Computed Cartesian Path
self._group.execute(plan)
# self.clear_octomap()

logical_camera_callback1(self, data):

docstring

Callback function of Logical Camera 2 topic.

Parameters:
        data: Message publishing on this topic

code

global LIST_A
model = data.models[1].type
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.stamp = rospy.Time.now()
object_pose.header.frame_id = "logical_camera_2_frame"
object_pose.pose.position.x = data.models[1].pose.position.x
object_pose.pose.position.y = data.models[1].pose.position.y
object_pose.pose.position.z = data.models[1].pose.position.z
object_pose.pose.orientation.x = data.models[1].pose.orientation.x
object_pose.pose.orientation.y = data.models[1].pose.orientation.y
object_pose.pose.orientation.z = data.models[1].pose.orientation.z
object_pose.pose.orientation.w = data.models[1].pose.orientation.w
while True:
    try:
        object_world_pose = TF_BUFFER.transform(object_pose, "world")
        x_a = object_world_pose.pose.position.x
        y_a = object_world_pose.pose.position.y
        z_a = object_world_pose.pose.position.z
        break
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException,\
    tf2_ros.ExtrapolationException):
        continue

To sort the packages according to their model names

pkg_clr = ''
for i in reversed(LIST_A):
    model = data.models[1].type
    pkg_n, pkg_clr = i
    if(pkg_clr == 'red' and model == pkg_n):
        if y_a > -0.03 and y_a < 0.04:
            self.conveyor(0)
            self.pkg1(x_a, y_a, z_a)
    elif(pkg_clr == 'yellow' and model == pkg_n):
        if  y_a > -0.03 and y_a < 0.03:
            self.conveyor(0)
            self.pkg2(x_a, y_a, z_a)
    elif(pkg_clr == 'green' and model == pkg_n):
        if  y_a > -0.04 and y_a < 0.02:
            self.conveyor(0)
            self.pkg3(x_a, y_a, z_a)
    else:
        print 0

bin1_pose(self, bin_x, bin_y, bin_z):

docstring

Function to give the coordinates of the packages and their respective bin.

Parameters:
        bin_x: X-coordinate
        bin_y: Y-coordinate
        bin_z: Z-coordinate

code

box_length = 0.15               # Length of the Package
vacuum_gripper_width = 0.115    # Vacuum Gripper Width
ur5_2_home_pose = geometry_msgs.msg.Pose()
ur5_2_home_pose.position.x = bin_x
ur5_2_home_pose.position.y = bin_y -0.03
ur5_2_home_pose.position.z = bin_z + vacuum_gripper_width + (box_length/2)
ur5_2_home_pose.orientation.x = -0.5
ur5_2_home_pose.orientation.y = -0.5
ur5_2_home_pose.orientation.z = 0.5
ur5_2_home_pose.orientation.w = 0.5
self.hard_go_to_pose(ur5_2_home_pose, 5)

go_to_pose(self, arg_pose):

docstring

Function to make the arm go to a predefined pose.

Parameters:
        arg_pose: Target goal pose.

Return:
     flag_plan: "True" if the goal is reached or "False" if the goal is failed.

code

pose_values = self._group.get_current_pose().pose
print pose_values
rospy.loginfo('\033[94m' + ">>> Current Pose:" + '\033[0m')
rospy.loginfo(pose_values)
self._group.set_pose_target(arg_pose)
flag_plan = self._group.go(wait=True)  # wait=False for Async Move
pose_values = self._group.get_current_pose().pose
rospy.loginfo('\033[94m'+ ">>> Final Pose:"+ '\033[0m')
rospy.loginfo(pose_values)
list_joint_values = self._group.get_current_joint_values()
rospy.loginfo('\033[94m'+ ">>> Final Joint Values:"+ '\033[0m')
rospy.loginfo(list_joint_values)
if flag_plan == True:
    pass
# rospy.loginfo(
    #'\033[94m' + ">>> set_joint_angles() Success" + '\033[0m')
else:
    pass
    # rospy.logerr(
    #'\033[94m' + ">>> set_joint_angles() Failed." + '\033[0m')
return flag_plan

hard_go_to_pose(self, arg_pose, arg_max_attempts):

docstring

Function to make the arm go to a predefined pose. \If failed it will try again for a certain number of attempts.

Parameters:
        arg_pose: Target goal pose.
        arg_max_attempts: No of attempts it will try to reach \the designated pose in case of failure.

code

number_attempts = 0
flag_success = False
while ((number_attempts <= arg_max_attempts) and  (flag_success is False):
    number_attempts += 1
    flag_success = self.go_to_pose(arg_pose)
    rospy.logwarn("attempts: {}".format(number_attempts))

joint_angles(self, a_angle, b_angle, c_angle, d_angle, e_angle, f_angle):

docstring

Function to convert the input angle(in degrees) to radian using "math" library.

Parameters:
        a_angle, b_angle, c_angle, d_angle, e_angle, f_angle: Six 
        joint angle of ur5_2 arm.

code

lst_joint_angles_1 = [math.radians(a_angle*57.29577951),\math.radians(b_angle*57.29577951), math.radians(c_angle*57.29577951),\math.radians(d_angle*57.29577951), math.radians(e_angle*57.29577951),\math.radians(f_angle*57.29577951)]

self.hard_set_joint_angles(lst_joint_angles_1, 5)

service1(self, ur5_msg):

docstring

Function to to operate the Vacuum Gripper(using service).

Parameters:
        ur5_msg: "True" or "False" depending upon whether we want to switch on or off the Vacuum Gripper.

code

self.msg = ur5_msg
rospy.wait_for_service('/eyrc/vb/ur5/activate_vacuum_gripper/ur5_2')
sos_service = rospy.ServiceProxy('/eyrc/vb/ur5/activate_vacuum_gripper/ur5_2', \vacuumGripper)
sos = vacuumGripperRequest(self.msg)
result1 = sos_service(sos)
print result1

get_time_str(self):

docstring

Function to get the current date time parameter in the prescribed format.

Return:
     Current date time in prescribed format

code

timestamp = int(time.time())
value = datetime.datetime.fromtimestamp(timestamp)
str_time = value.strftime('%Y-%m-%d %H:%M:%S')
return str_time

msg_order_1(self):

docstring

Function to publish message on "/eyrc/vb/VsMyPsSr/spreadsheet/ordershipped"
  this topic.

code

msg_mqtt_sub = msg_spreadsheet_2()
msg_mqtt_sub.timestamp = rospy.Time.now()
msg_mqtt_sub.topic = "/eyrc/vb/VsMyPsSr/spreadsheet/ordershipped"
msg_mqtt_sub.time_shipped = self.get_time_str()
msg_mqtt_sub.message = str(payload)
msg_mqtt_sub.sim_time = str(simtime)
self._handle_ros_pub.publish(msg_mqtt_sub)

__ del __(self):

docstring

Destructor function to delete the class object.

code

moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class Ur5Moveit Deleted." + '\033[0m')

main():

docstring

This is the main function to initiate the ros node ur5_2

code

global TF_BUFFER
global TF_LISTENER
global SUB_ONCE
TF_BUFFER = tf2_ros.Buffer(rospy.Time(0.5))
TF_LISTENER = tf2_ros.TransformListener(TF_BUFFER)
ur5 = CartesianPath("ur5_2")
rospy.sleep(6)
ur5.conveyor(100)
thread_ur5 = threading.Thread(target=ur5.bin1_pose, args=[-0.8, 0, 1])
thread_ur5.start()
SUB_ONCE = (rospy.Subscriber("/eyrc/vb/camera_1/image_raw",\
Image, ur5.camera_1_callback))
while not rospy.is_shutdown():
rospy.Subscriber('/eyrc/vb/logical_camera_2', LogicalCameraImage\
, ur5.logical_camera_callback1, queue_size=1)
rospy.Subscriber("/eyrc/vb/VsMyPsSr/spreadsheet/ordersdispatch",\
msg_spreadsheet_1, ur5.dispatch_orders, queue_size=3)
rospy.spin()
cv2.destroyAllWindows()
del ur5