services.py

Go to the documentation of this file.

#!/usr/bin/env python
from robodk_postprocessors.Fanuc_R30iA import RobotPost as FanucR30iAPost
from robodk_postprocessors.Motoman import Pose
from robodk_postprocessors.Motoman import RobotPost as MotomanPost
from robodk_postprocessors.robodk import *
from ros_robodk_post_processors.srv import *
import config
import geometry_msgs.msg
import motoman
import rospy

# From geometry_msgs.Pose to RoboDK.Mat
def poseToMat(p):
    # Warning: Quaternion initialization order: w, x, y, z
    quat = [p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z]
    mat = quaternion_2_pose(quat)
    mat.setPos([p.position.x * 1000, p.position.y * 1000, p.position.z * 1000])
    return mat

# Check if pose is initialized
def poseInitialized(p):
    if p.orientation.x == 0 and p.orientation.y == 0 and p.orientation.z == 0 and p.orientation.w == 0:
        return False
    return True

def move_c(req):
    if config.pp is None:
        return [config.pp_not_init]

    if len(req.joints_1) != config.pp.nAxes:
        return ["Joints tuple does not match the number of axes"]
    if len(req.joints_2) != config.pp.nAxes:
        return ["Joints tuple does not match the number of axes"]
    if len(req.conf_RLF_1) != 3:
        return ["conf_RLF_1 size must be 3"]
    if len(req.conf_RLF_2) != 3:
        return ["conf_RLF_2 size must be 3"]

    if not poseInitialized(req.pose_1):
        return ["Pose 1 quaternions are not initialized"]
    if not poseInitialized(req.pose_2):
        return ["Pose 2 quaternions are not initialized"]
    p1 = poseToMat(req.pose_1)
    p2 = poseToMat(req.pose_2)
    config.pp.MoveC(p1, req.joints_1, p2, req.joints_2, list(req.conf_RLF_1), list(req.conf_RLF_2))
    return [""]

def move_j(req):
    if config.pp is None:
        return [config.pp_not_init]

    if len(req.joints) != config.pp.nAxes:
        return ["Joints tuple does not match the number of axes"]
    if len(req.conf_RLF) != 3:
        return ["conf_RLF size must be 3"]

    if not poseInitialized(req.pose):
        return ["Pose quaternions are not initialized"]
    p = poseToMat(req.pose)
    config.pp.MoveJ(p, req.joints, list(req.conf_RLF))
    return [""]

def move_l(req):
    if config.pp is None:
        return [config.pp_not_init]

    if len(req.joints) != config.pp.nAxes:
        return ["Joints tuple does not match the number of axes"]
    if len(req.conf_RLF) != 3:
        return ["conf_RLF size must be 3"]

    if not poseInitialized(req.pose):
        return ["Pose quaternions are not initialized"]

    p = poseToMat(req.pose)
    config.pp.MoveL(p, req.joints, list(req.conf_RLF))
    return [""]

def pause(req):
    if config.pp is None:
        return [config.pp_not_init]

    if req.seconds <= 0.0:
        return ["Pause cannot be zero or negative"];

    config.pp.Pause(req.seconds * 1000)
    return [""]

def prog_finish(req):
    if config.pp is None:
        return [config.pp_not_init]

    config.pp.ProgFinish(req.program_name)
    if len(config.pp.LOG) > 0:
        return [config.pp.LOG, ""]

    program=''
    for line in config.pp.PROG_LIST[-1]:
        program += line
    return ["", program]

def prog_save(req):
    if config.pp is None:
        return [config.pp_not_init, ""]

    if len(config.pp.PROG_LIST) is 0:
        return ["Program list is empty", ""]

    program=''
    for line in config.pp.PROG_LIST[-1]:
        program += line

    if not req.program_name:
        return ["program_name cannot be empty", ""]
    if not req.file_saving_dir:
        return ["file_saving_dir cannot be empty", ""]
    config.pp.ProgSave(req.file_saving_dir, req.program_name)
    return ["", program]

def prog_send_robot(req):
    if config.pp is None:
        return [config.pp_not_init, ""]

    if not req.robot_ip:
        return ["robot_ip cannot be empty"]

    config.pp.ProgSendRobot(req.robot_ip, req.remote_path, req.ftp_user, req.ftp_pass)
    return [""]

def prog_start(req):
    if len(req.post_processor) is 0:
        return ["Post processor name is empty"]
    elif req.post_processor == "Motoman":
        config.pp = MotomanPost()
        config.pp.LAST_CONFDATA = [None, None, None, None] # Reset last configuration
    elif req.post_processor == "Fanuc_R30iA":
        config.pp = FanucR30iAPost()
    # New post-processors go here
    else:
        return ["'%s' post processor is not supported" % req.post_processor]

    config.pp.PROG_TARGETS = []

    if len(req.program_name) is 0:
        return ["Program name is empty"]
    config.pp.ProgStart(req.program_name)

    # Program comment is allowed to be empty
    config.pp.PROG_COMMENT = req.program_comment

    return [""]

def run_message(req):
    if config.pp is None:
        return [config.pp_not_init]

    if not req.msg:
        return ["message cannot be empty"]

    config.pp.RunMessage(req.msg)
    return [""]

def run_code(req):
    if config.pp is None:
        return [config.pp_not_init]

    if not req.code:
        return ["code cannot be empty"]

    config.pp.RunCode(req.code, req.is_function_call)
    return [""]

def set_do(req): # set Digital Output
    if config.pp is None:
        return [config.pp_not_init]

    try:
        var = int(req.io_var)
        config.pp.setDO(var, req.io_value)
    except ValueError:
        config.pp.setDO(req.io_var, req.io_value)

    return [""]

def set_frame(req):
    if config.pp is None:
        return [config.pp_not_init]

    if not poseInitialized(req.pose):
        return ["Pose quaternions are not initialized"]
    p = poseToMat(req.pose)
    config.pp.setFrame(p, req.frame_id, req.frame_name)
    return [""]

def set_speed(req):
    if config.pp is None:
        return [config.pp_not_init]

    config.pp.setSpeed(req.mm_sec)
    return [""]

def set_speed_joints(req):
    if config.pp is None:
        return [config.pp_not_init]

    config.pp.setSpeedJoints(req.deg_sec)
    return [""]

def set_tool(req):
    if config.pp is None:
        return [config.pp_not_init]

    if not poseInitialized(req.pose):
        return ["Pose quaternions are not initialized"]
    p = poseToMat(req.pose)
    config.pp.setTool(p, req.tool_id, req.tool_name)
    return [""]

def set_zone_data(req):
    if config.pp is None:
        return [config.pp_not_init]

    config.pp.setZoneData(req.zone_mm)
    return [""]

def wait_di(req):
    if config.pp is None:
        return [config.pp_not_init]

    try:
        var = int(req.io_var)
        config.pp.waitDI(var, req.io_value, req.timeout_ms)
    except ValueError:
        config.pp.waitDI(req.io_var, req.io_value, req.timeout_ms)

    return [""]

# Common services
def services_servers():
    rospy.init_node('ros_robodk_post_processors')
    service_prefix = 'robodk_post_processors/'
    services = []

    # Common services to all post-processors
    services.append(rospy.Service(service_prefix + 'move_c', MoveC, move_c))
    services.append(rospy.Service(service_prefix + 'move_j', MoveJ, move_j))
    services.append(rospy.Service(service_prefix + 'move_l', MoveL, move_l))
    services.append(rospy.Service(service_prefix + 'pause', Pause, pause))
    services.append(rospy.Service(service_prefix + 'prog_finish', ProgFinish, prog_finish))
    services.append(rospy.Service(service_prefix + 'prog_save', ProgSave, prog_save))
    services.append(rospy.Service(service_prefix + 'prog_send_robot', ProgSendRobot, prog_send_robot))
    services.append(rospy.Service(service_prefix + 'prog_start', ProgStart, prog_start))
    services.append(rospy.Service(service_prefix + 'run_code', RunCode, run_code))
    services.append(rospy.Service(service_prefix + 'run_message', RunMessage, run_message))
    services.append(rospy.Service(service_prefix + 'set_do', SetDO, set_do))
    services.append(rospy.Service(service_prefix + 'set_frame', SetFrame, set_frame))
    services.append(rospy.Service(service_prefix + 'set_speed', SetSpeed, set_speed))
    services.append(rospy.Service(service_prefix + 'set_speed_joints', SetSpeedJoints, set_speed_joints))
    services.append(rospy.Service(service_prefix + 'set_tool', SetTool, set_tool))
    services.append(rospy.Service(service_prefix + 'set_zone_data', SetZoneData, set_zone_data))
    services.append(rospy.Service(service_prefix + 'wait_di', WaitDI, wait_di))

    # Not implemented:
    # setSpeed
    # setAcceleration
    # setSpeedJoints
    # setAccelerationJoints
    # setZoneData

    # Brand specific services
    motoman.services(service_prefix, services)

    # Display all services advertised
    for s in services:
        rospy.loginfo("Service %s ready", s.resolved_name)
    rospy.spin()

if __name__ == '__main__':
    services_servers()