ROS 2 control stack for Dobot Magician educational manipulator.
import rclpy
from rclpy.node import Node
from dobot_driver.dobot_handle import bot


class SetToolNullConfigurator(Node):

    def __init__(self):
        super().__init__('dobot_init_config')
        print("Loading the initial configuration into the robot.")
        bot.set_end_effector_params(0, 0, 0)

def main(args=None):
    rclpy.init(args=args)

    startup_params_configurator = SetToolNullConfigurator()

    startup_params_configurator.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_bringup/dobot_bringup/set_tool_null.py
jkaniuka/magician_ros2
import os, sys, subprocess
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import LogInfo, RegisterEventHandler, TimerAction, ExecuteProcess
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessStart, OnShutdown
from launch.substitutions import LocalSubstitution, PythonExpression


def generate_launch_description():

    # -----------------------------------------------------------------------------------------------------------------
    # Check if the robot is physically connected

    shell_cmd = subprocess.Popen('lsusb | grep "Silicon Labs CP210x UART Bridge" ', shell=True, stdout=subprocess.PIPE)
    is_connected = shell_cmd.stdout.read().decode('utf-8')
    if not is_connected:
        sys.exit("Dobot is disconnected!")
    # -----------------------------------------------------------------------------------------------------------------


    # -----------------------------------------------------------------------------------------------------------------
    # Check if MAGICIAN_TOOL env var has the right value 
    tool_env_var = str(os.environ.get('MAGICIAN_TOOL'))

    if tool_env_var == "None":
         sys.exit("MAGICIAN_TOOL env var is not set!")

    if tool_env_var not in ['none', 'pen', 'suction_cup', 'gripper', 'extended_gripper']:
         sys.exit("MAGICIAN_TOOL env var has an incorrect value!")

    valid_tool=["'", tool_env_var, "' == 'none'", 'or', \
                "'", tool_env_var, "' == 'pen'", 'or', \
                "'", tool_env_var, "' == 'suction_cup'", 'or', \
                "'", tool_env_var, "' == 'gripper'", 'or', \
                "'", tool_env_var, "' == 'extended_gripper'"
    ]
    # -----------------------------------------------------------------------------------------------------------------


    # -----------------------------------------------------------------------------------------------------------------
    # Nodes & launch files

    tool_null = Node(
        package='dobot_bringup',
        executable='set_tool_null',
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    alarms =ExecuteProcess(
        cmd=[[
            'ros2 ', 'launch ', 'dobot_diagnostics ', 'alarms_analyzer.launch.py'
        ]],
        shell=True,
        output='log',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    gripper = Node(
        package='dobot_end_effector',
        executable='gripper_server',
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    suction_cup = Node(
        package='dobot_end_effector',
        executable='suction_cup_server',
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    homing =ExecuteProcess(
        cmd=[[
            'ros2 ', 'launch ', 'dobot_homing ', 'dobot_homing.launch.py'
        ]],
        shell=True,
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    trajectory_validator =ExecuteProcess(
        cmd=[[
            'ros2 ', 'launch ', 'dobot_kinematics ', 'dobot_validate_trajectory.launch.py'
        ]],
        shell=True,
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    PTP_action =ExecuteProcess(
        cmd=[[
            'ros2 ', 'launch ', 'dobot_motion ', 'dobot_PTP.launch.py'
        ]],
        shell=True,
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    robot_state = Node(
        package='dobot_state_updater',
        executable='state_publisher',
        output='screen',
        condition = IfCondition(PythonExpression(valid_tool))
    )

    # -----------------------------------------------------------------------------------------------------------------

    # -----------------------------------------------------------------------------------------------------------------
    # OnProcessStart events for information purposes 

    tool_null_event = RegisterEventHandler(
        OnProcessStart(
            target_action=tool_null,
            on_start=[
                LogInfo(msg='Loading tool parameters.')
            ]
        )
    )

    alarms_event = RegisterEventHandler(
        OnProcessStart(
            target_action=alarms,
            on_start=[
                LogInfo(msg='Starting the diagnostics module.')
            ]
        )
    )

    gripper_event = RegisterEventHandler(
        OnProcessStart(
            target_action=gripper,
            on_start=[
                LogInfo(msg='Gripper control service started.')
            ]
        )
    )

    suction_cup_event = RegisterEventHandler(
        OnProcessStart(
            target_action=suction_cup,
            on_start=[
                LogInfo(msg='Suction Cup control service started.')
            ]
        )
    )

    homing_event = RegisterEventHandler(
        OnProcessStart(
            target_action=homing,
            on_start=[
                LogInfo(msg='Starting homing service.'),
                LogInfo(msg='Loading homing parameters.')
            ]
        )
    )

    trajectory_validator_event = RegisterEventHandler(
        OnProcessStart(
            target_action=trajectory_validator,
            on_start=[
                LogInfo(msg='Strating trajectory validator service.'),
                LogInfo(msg='Loading kinematics parameters.')
            ]
        )
    )
    PTP_action_event = RegisterEventHandler(
        OnProcessStart(
            target_action=PTP_action,
            on_start=[
                LogInfo(msg='Starting PointToPoint action server.'),
                LogInfo(msg='Setting speed and acceleration values.')
            ]
        )
    )

    robot_state_event = RegisterEventHandler(
        OnProcessStart(
            target_action=robot_state,
            on_start=[
                LogInfo(msg='Dobot state updater node started.'),
                LogInfo(msg='Dobot Magician control stack has been launched correctly')
            ]
        )
    )
    # -----------------------------------------------------------------------------------------------------------------



    # -----------------------------------------------------------------------------------------------------------------
    # Shutdown event handler
    on_shutdown = RegisterEventHandler(
        OnShutdown(
            on_shutdown=[LogInfo(
                msg=['Dobot Magician control system launch was asked to shutdown: ',
                    LocalSubstitution('event.reason')]
            )]
        )
    )
    # -----------------------------------------------------------------------------------------------------------------

    # -----------------------------------------------------------------------------------------------------------------
    # Launch scheduler


    tool_null_sched = TimerAction(
        period=1.0,
        actions=[tool_null]
        )

    alarms_sched = TimerAction(
        period=2.0,
        actions=[alarms]
        )

    gripper_sched = TimerAction(
        period=2.0,
        actions=[gripper]
        )

    suction_cup_sched = TimerAction(
        period=2.0,
        actions=[suction_cup]
        )

    homing_sched = TimerAction(
        period=3.0,
        actions=[homing]
        )

    trajectory_validator_sched = TimerAction(
        period=5.0,
        actions=[trajectory_validator]
        )

    PTP_action_sched = TimerAction(
        period=7.0,
        actions=[PTP_action]
        )

    robot_state_sched = TimerAction(
        period=18.0,
        actions=[robot_state]
        )

    # -----------------------------------------------------------------------------------------------------------------


    return LaunchDescription([
        tool_null_event,
        alarms_event,
        gripper_event,
        suction_cup_event,
        homing_event,
        trajectory_validator_event,
        PTP_action_event,
        robot_state_event,
        tool_null_sched,
        alarms_sched,
        gripper_sched,
        suction_cup_sched,
        homing_sched,
        trajectory_validator_sched,
        PTP_action_sched,
        robot_state_sched,
        on_shutdown
    ])
dobot_bringup/launch/dobot_magician_control_system.launch.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_bringup'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name), glob('launch/*.launch.py')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Bringup package for Dobot Magician',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'set_tool_null = dobot_bringup.set_tool_null:main',
        ],
    },
)

dobot_bringup/setup.py
jkaniuka/magician_ros2
#!/usr/bin/env python

from __future__ import division

import itertools
import os
from std_msgs.msg import String
from dobot_driver.dobot_handle import bot

from ament_index_python import get_resource
from python_qt_binding import loadUi
from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot, QProcess
from python_qt_binding.QtGui import QIcon
from python_qt_binding.QtWidgets import QHeaderView, QMenu, QTreeWidgetItem, QWidget, QVBoxLayout, QSizePolicy
from rqt_py_common.message_helpers import get_message_class
from PyQt5.QtWidgets import QMessageBox
from PyQt5.QtGui import *
from PyQt5.QtCore import *
import subprocess
import rclpy
from geometry_msgs.msg import PoseStamped 
import math
import tf_transformations
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
from dobot_msgs.msg import GripperStatus
import sys




class DobotControlPanel(QWidget):

    def __init__(self, node, plugin=None):

        super(DobotControlPanel, self).__init__()

        shell_cmd = subprocess.Popen('lsusb | grep "Silicon Labs CP210x UART Bridge" ', shell=True, stdout=subprocess.PIPE)
        is_connected = shell_cmd.stdout.read().decode('utf-8')
        if not is_connected:
            sys.exit("Dobot is disconnected!")


        self._node = node
        self._logText = ''
        self.process  = QProcess(self)
        self._plugin = plugin

        _, package_path = get_resource('packages', 'dobot_control_panel')
        ui_file = os.path.join(package_path, 'share', 'dobot_control_panel', 'resource', 'DobotControlPanel_layout.ui')
        loadUi(ui_file, self)


        self.subscription_TCP = self._node.create_subscription(
            PoseStamped,
            'dobot_TCP',
            self.tcp_position_callback,
            10)

        self.subscription_joints = self._node.create_subscription(
            JointState,
            'dobot_joint_states',
            self.joints_positions_callback,
            10)

        self.gripper_state_publ = self._node.create_publisher(GripperStatus, 'gripper_status_rviz', 10)

        self.dobot_current_joint_states = []

        if os.environ.get('MAGICIAN_RAIL_IN_USE') == "true":
            self.RAIL_IN_USE = True
            bot.set_sliding_rail_status(1,1)
            bot.set_point_to_point_sliding_rail_params(100, 100)
            bot.set_point_to_point_sliding_rail_params(100*2, 100*2) #BUG
            self.subscription_rail_pose = self._node.create_subscription(
                Float64,
                'dobot_rail_pose',
                self.rail_pose_callback,
                10)
        else:
            self.RAIL_IN_USE = False
            self.CurrentPositionRail.setText(str("X"))

        # Control Tab
        self.JT1Plus.pressed.connect(lambda:self.JT1_move(self.JT1Plus))
        self.JT1Plus.released.connect(self.JT_IDLE)
        self.JT1Minus.pressed.connect(lambda:self.JT1_move(self.JT1Minus))
        self.JT1Minus.released.connect(self.JT_IDLE)
        
        self.JT2Plus.pressed.connect(lambda:self.JT2_move(self.JT2Plus))
        self.JT2Plus.released.connect(self.JT_IDLE)
        self.JT2Minus.pressed.connect(lambda:self.JT2_move(self.JT2Minus))
        self.JT2Minus.released.connect(self.JT_IDLE)

        self.JT3Plus.pressed.connect(lambda:self.JT3_move(self.JT3Plus))
        self.JT3Plus.released.connect(self.JT_IDLE)
        self.JT3Minus.pressed.connect(lambda:self.JT3_move(self.JT3Minus))
        self.JT3Minus.released.connect(self.JT_IDLE)

        self.JT4Plus.pressed.connect(lambda:self.JT4_move(self.JT4Plus))
        self.JT4Plus.released.connect(self.JT_IDLE)
        self.JT4Minus.pressed.connect(lambda:self.JT4_move(self.JT4Minus))
        self.JT4Minus.released.connect(self.JT_IDLE)

        self.HomingButton.clicked.connect(self.button_clicked_HomingButton)
        self.EStopButton.clicked.connect(self.button_clicked_EStopButton)

        self.BaseFrame.setChecked(True)
        self.frame = "base"
        self.BaseFrame.toggled.connect(lambda:self.framestate(self.BaseFrame))
        self.JointFrame.toggled.connect(lambda:self.framestate(self.JointFrame))


        # Tool Tab
        self.GripperOpen.pressed.connect(self.open_gripper)
        self.GripperClose.pressed.connect(self.close_gripper)
        self.SuctionCupTurnOn.pressed.connect(self.turn_on_suction_cup)
        self.SuctionCupTurnOff.pressed.connect(self.turn_off_suction_cup)

        # Speed Tab
        self.Joint1VelSlider.valueChanged.connect(lambda:self.valuechange_joints(self.JT1Vel))
        self.Joint2VelSlider.valueChanged.connect(lambda:self.valuechange_joints(self.JT2Vel))
        self.Joint3VelSlider.valueChanged.connect(lambda:self.valuechange_joints(self.JT3Vel))
        self.Joint4VelSlider.valueChanged.connect(lambda:self.valuechange_joints(self.JT4Vel))

        self.XVelSlider.valueChanged.connect(lambda:self.valuechange_cartesian(self.XVel))
        self.YVelSlider.valueChanged.connect(lambda:self.valuechange_cartesian(self.YVel))
        self.ZVelSlider.valueChanged.connect(lambda:self.valuechange_cartesian(self.ZVel))
        self.RVelSlider.valueChanged.connect(lambda:self.valuechange_cartesian(self.RVel))

        # -----------------------------------------------------------------------------

        self.Joint1VelSlider.sliderReleased.connect(lambda:self.change_vel_joints(self.Joint1VelSlider))
        self.Joint2VelSlider.sliderReleased.connect(lambda:self.change_vel_joints(self.Joint2VelSlider))
        self.Joint3VelSlider.sliderReleased.connect(lambda:self.change_vel_joints(self.Joint3VelSlider))
        self.Joint4VelSlider.sliderReleased.connect(lambda:self.change_vel_joints(self.Joint4VelSlider))

        self.XVelSlider.sliderReleased.connect(lambda:self.change_vel_cartesian(self.XVelSlider))
        self.YVelSlider.sliderReleased.connect(lambda:self.change_vel_cartesian(self.YVelSlider))
        self.ZVelSlider.sliderReleased.connect(lambda:self.change_vel_cartesian(self.ZVelSlider))
        self.RVelSlider.sliderReleased.connect(lambda:self.change_vel_cartesian(self.RVelSlider))

        # -----------------------------------------------------------------------------

        self.RailSVSlider.sliderReleased.connect(self.sliding_rail_SV_slider)
        self.RailSVSlider.valueChanged.connect(self.sliding_rail_current_pose)

        self.RailVelocitySlider.sliderReleased.connect(self.sliding_rail_params_slider)
        self.RailVelocitySlider.valueChanged.connect(self.sliding_rail_vel_display)

        self.RailAccelerationSlider.sliderReleased.connect(self.sliding_rail_params_slider)
        self.RailAccelerationSlider.valueChanged.connect(self.sliding_rail_acc_display)

        self.StopRailButton.clicked.connect(self.button_clicked_StopRail)




        # initial setup
        bot.set_jog_common_params(100, 1) # (vel_ratio, acc_ratio)
        bot.set_jog_joint_params([100, 100, 100, 100], [100, 100, 100, 100])
        bot.set_jog_coordinate_params([100, 100, 100, 100], [100, 100, 100, 100])




    def tcp_position_callback(self, msg):
        self.XPoseLCD.setText(str(round((msg.pose.position.x)*1000, 3)))
        self.YPoseLCD.setText(str(round((msg.pose.position.y)*1000, 3)))
        self.ZPoseLCD.setText(str(round((msg.pose.position.z)*1000, 3)))

        quat = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
        _, _, y = tf_transformations.euler_from_quaternion(quat)

        self.RPoseLCD.setText(str(round((math.degrees(y)), 3)))


    def joints_positions_callback(self, msg):
        self.dobot_current_joint_states = [math.degrees(msg.position[0]), math.degrees(msg.position[1]), math.degrees(msg.position[2]), math.degrees(msg.position[3])]
        self.JT1LCD.setText(str(round((math.degrees(msg.position[0])), 3)))
        self.JT2LCD.setText(str(round((math.degrees(msg.position[1])), 3)))
        self.JT3LCD.setText(str(round((math.degrees(msg.position[2])), 3)))
        self.JT4LCD.setText(str(round((math.degrees(msg.position[3])), 3)))

    def rail_pose_callback(self, msg):
        self.rail_pose = msg.data
        self.CurrentPositionRail.setText(str(int(self.rail_pose)))


    def framestate(self, b):
        
        if b.text() == "Base":
            if b.isChecked() == True:
                self.frame = "base"
                    
        if b.text() == "Joint":
            if b.isChecked() == True:
                self.frame = "joint"

    def JT_IDLE(self):
        if self.frame == "base":
            bot.set_jog_command(0, 0)
        elif self.frame == "joint":
            bot.set_jog_command(1, 0)

        
    def JT1_move(self, sign):
        if sign.text() == "+":
            if self.frame == "base":
                bot.set_jog_command(0, 1)
            elif self.frame == "joint":
                bot.set_jog_command(1, 1)
        elif sign.text() == "-":
            if self.frame == "base":
                bot.set_jog_command(0, 2)
            elif self.frame == "joint":
                bot.set_jog_command(1, 2)

    def JT2_move(self, sign):
        if sign.text() == "+":
            if self.frame == "base":
                bot.set_jog_command(0, 3)
            elif self.frame == "joint":
                bot.set_jog_command(1, 3)
        elif sign.text() == "-":
            if self.frame == "base":
                bot.set_jog_command(0, 4)
            elif self.frame == "joint":
                bot.set_jog_command(1, 4)

    def JT3_move(self, sign):
        if sign.text() == "+":
            if self.frame == "base":
                bot.set_jog_command(0, 5)
            elif self.frame == "joint":
                bot.set_jog_command(1, 5)
        elif sign.text() == "-":
            if self.frame == "base":
                bot.set_jog_command(0, 6)
            elif self.frame == "joint":
                bot.set_jog_command(1, 6)

    def JT4_move(self, sign):
        if sign.text() == "+":
            if self.frame == "base":
                bot.set_jog_command(0, 7)
            elif self.frame == "joint":
                bot.set_jog_command(1, 7)
        elif sign.text() == "-":
            if self.frame == "base":
                bot.set_jog_command(0, 8)
            elif self.frame == "joint":
                bot.set_jog_command(1, 8)

    def button_clicked_HomingButton(self):   
        msg = QMessageBox()
        msg.setIcon(QMessageBox.Information)

        # msg.setText("Information")
        msg.setInformativeText("The homing procedure is about to start. Wait until the arm stops moving and the led stops flashing blue and turns green.")
        msg.setWindowTitle("Homing procedure")
        msg.setStandardButtons(QMessageBox.Ok)
        msg.exec_()

        command = 'ros2 service call /dobot_homing_service dobot_msgs/srv/ExecuteHomingProcedure'
        subprocess.Popen(
                    command, universal_newlines=True, shell=True,
                    stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

    def button_clicked_EStopButton(self):
        bot.stop_queue(force=True) 

        command = 'ros2 service call /PTP_action/_action/cancel_goal action_msgs/srv/CancelGoal'
        subprocess.Popen(
                    command, universal_newlines=True, shell=True,
                    stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

        msg = QMessageBox()
        msg.setIcon(QMessageBox.Warning)

        msg.setText("Warning")
        msg.setInformativeText("The stop button has been pressed. Make sure nothing is in the robot's workspace before resuming operation.")
        msg.setWindowTitle("Motion stop")
        msg.setStandardButtons(QMessageBox.Ok)
        msg.exec_()


    def open_gripper(self):
        command = 'ros2 service call /dobot_gripper_service dobot_msgs/srv/GripperControl "{gripper_state: "open", keep_compressor_running: false}"'
        subprocess.Popen(
                    command, universal_newlines=True, shell=True,
                    stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

        self.send_gripper_state("opened")
 
    def close_gripper(self):
        command = 'ros2 service call /dobot_gripper_service dobot_msgs/srv/GripperControl "{gripper_state: "close", keep_compressor_running: false}"'
        subprocess.Popen(
                    command, universal_newlines=True, shell=True,
                    stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

        self.send_gripper_state("closed")


    def turn_on_suction_cup(self):
        command = 'ros2 service call /dobot_suction_cup_service dobot_msgs/srv/SuctionCupControl "{enable_suction: true}"'
        subprocess.Popen(
            command, universal_newlines=True, shell=True,
            stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

    def turn_off_suction_cup(self):
        command = 'ros2 service call /dobot_suction_cup_service dobot_msgs/srv/SuctionCupControl "{enable_suction: false}"'
        subprocess.Popen(
            command, universal_newlines=True, shell=True,
            stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

    def valuechange_joints(self, field):
        if field.objectName() == "JT1Vel":
            self.JT1Vel.setText(str(self.Joint1VelSlider.value()))
        elif field.objectName() == "JT2Vel":
            self.JT2Vel.setText(str(self.Joint2VelSlider.value()))
        elif field.objectName() == "JT3Vel":
            self.JT3Vel.setText(str(self.Joint3VelSlider.value()))
        elif field.objectName() == "JT4Vel":
            self.JT4Vel.setText(str(self.Joint4VelSlider.value()))


    def valuechange_cartesian(self, field):
        if field.objectName() == "XVel":
            self.XVel.setText(str(self.XVelSlider.value()))
        elif field.objectName() == "YVel":
            self.YVel.setText(str(self.YVelSlider.value()))
        elif field.objectName() == "ZVel":
            self.ZVel.setText(str(self.ZVelSlider.value()))
        elif field.objectName() == "RVel":
            self.RVel.setText(str(self.RVelSlider.value()))

    def change_vel_joints(self, slider):
        bot.set_jog_joint_params([int(self.Joint1VelSlider.value()), int(self.Joint2VelSlider.value()), int(self.Joint3VelSlider.value()), int(self.Joint4VelSlider.value())], [100, 100, 100, 100])


    def change_vel_cartesian(self, slider):
        bot.set_jog_coordinate_params([int(self.XVelSlider.value()), int(self.YVelSlider.value()), int(self.ZVelSlider.value()), int(self.RVelSlider.value())], [100, 100, 100, 100])


    def sliding_rail_disconnected(self):
        msg = QMessageBox()
        msg.setIcon(QMessageBox.Warning)

        msg.setText("Sliding rail is not connected")
        msg.setInformativeText("If you have actually placed the manipulator on the sliding rail, you must additionally set MAGICIAN_RAIL_IN_USE environment variable to 'true'.")
        msg.setWindowTitle("WARNING")
        msg.setStandardButtons(QMessageBox.Close)
        msg.exec_()

    def sliding_rail_SV_slider(self):
        if not self.RAIL_IN_USE:
            self.sliding_rail_disconnected()
            return

        bot.set_point_to_point_sliding_rail_command(4,self.dobot_current_joint_states[0], self.dobot_current_joint_states[1], self.dobot_current_joint_states[2], self.dobot_current_joint_states[3], int(self.RailSVSlider.value()))

    def sliding_rail_current_pose(self):
        self.RailSVDisplay.setText(str(self.RailSVSlider.value()))

    def sliding_rail_params_slider(self):
        if not self.RAIL_IN_USE:
            self.sliding_rail_disconnected()
            return
        bot.set_point_to_point_sliding_rail_params(int(self.RailVelocitySlider.value()) * 2, int(self.RailAccelerationSlider.value()) * 2) #BUG

    def sliding_rail_vel_display(self):
        self.RailVelLCD.setText(str(self.RailVelocitySlider.value()))


    def sliding_rail_acc_display(self):
        self.RailAccLCD.setText(str(self.RailAccelerationSlider.value()))

    def button_clicked_StopRail(self):
        if not self.RAIL_IN_USE:
            self.sliding_rail_disconnected()
            return
        bot.stop_queue(force=True) 
        bot.clear_queue()
        bot.start_queue()

        msg = QMessageBox()
        msg.setIcon(QMessageBox.Warning)

        msg.setText("Warning")
        msg.setInformativeText("The stop button has been pressed. Make sure nothing is in the robot's workspace before resuming operation.")
        msg.setWindowTitle("Motion stop")
        msg.setStandardButtons(QMessageBox.Ok)
        msg.exec_()

    # --------------------------------------------------------
    def send_gripper_state(self, state):
        msg = GripperStatus()
        msg.header.stamp = self._node.get_clock().now().to_msg()
        msg.status = state
        self.gripper_state_publ.publish(msg)


          
    def start(self):
        pass

    def shutdown_plugin(self):
        pass

dobot_control_panel/dobot_control_panel/dobot_control_panel_widget.py
jkaniuka/magician_ros2
#!/usr/bin/env python3

import sys
from rqt_gui.main import Main

def main():
    main = Main()
    sys.exit(main.main(sys.argv, standalone='dobot_control_panel.ros2_dobot_control_panel.Ros2DobotControlPanel'))

if __name__ == '__main__':
    main()
dobot_control_panel/dobot_control_panel/main.py
jkaniuka/magician_ros2
from rqt_gui_py.plugin import Plugin
from .dobot_control_panel_widget import DobotControlPanel
    
    
class Ros2DobotControlPanel(Plugin):

    def __init__(self, context):
        super(Ros2DobotControlPanel, self).__init__(context)
        self._node = context.node
        self._logger = self._node.get_logger().get_child('dobot_control_panel.ros2_dobot_control_panel.Ros2DobotControlPanel')
        
        super(Ros2DobotControlPanel, self).__init__(context)
        self.setObjectName('Ros2DobotControlPanel')

        self._widget = DobotControlPanel(context.node, self)

        self._widget.start()
        if context.serial_number() > 1:
            self._widget.setWindowTitle(
                self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

dobot_control_panel/dobot_control_panel/ros2_dobot_control_panel.py
jkaniuka/magician_ros2
from setuptools import setup
from glob import glob
import os

package_name = 'dobot_control_panel'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/resource', ['resource/DobotControlPanel_layout.ui']),
        ('share/' + package_name, ['plugin.xml']),
        ('share/' + package_name + '/resource/images', glob('resource/images/*.png')),
        ('share/' + package_name + '/resource/images', glob('resource/images/*.svg'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='RQT-based GUI for Dobot Magician',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'dobot_control_panel = dobot_control_panel.main:main',
        ],
    },
)


dobot_control_panel/setup.py
jkaniuka/magician_ros2
from dobot_msgs.action import PointToPoint
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
import time


class DobotPTPClient(Node):

    def __init__(self):
        super().__init__('dobot_PTP_client')
        self._action_client = ActionClient(self, PointToPoint, 'PTP_action')


    def cancel_done(self, future): 
        cancel_response = future.result()
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info('Goal successfully canceled')
        else:
            self.get_logger().info('Goal failed to cancel')
        rclpy.shutdown()

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self._goal_handle = goal_handle

        self.get_logger().info('Goal accepted :)')

        # Start a 0.5 second timer
        # self._timer = self.create_timer(0.5, self.timer_callback)

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Result: {0}'.format(result))
        rclpy.shutdown()

    def feedback_callback(self, feedback):
        self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.current_pose))

    def timer_callback(self):
        self.get_logger().info('Canceling goal')
        # Cancel the goal
        future = self._goal_handle.cancel_goal_async()
        future.add_done_callback(self.cancel_done)

        # Cancel the timer
        self._timer.cancel()

    def send_goal(self, target, mode):
        self.get_logger().info('Waiting for action server...')
        self._action_client.wait_for_server()

        goal_msg = PointToPoint.Goal()
        goal_msg.target_pose = target
        goal_msg.motion_type = mode
        self.get_logger().info('Sending goal request...')

        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)


def main(args=None):
    rclpy.init(args=args)

    action_client = DobotPTPClient()

    action_client.send_goal(target = [200.0, 0.0, 100.0, 0.0], mode = 1)

    rclpy.spin(action_client)


if __name__ == '__main__':
    main()

dobot_demos/dobot_demos/PTP_client.py
jkaniuka/magician_ros2
from dobot_msgs.srv import GripperControl
import rclpy
from rclpy.node import Node


class GripperClient(Node):

    def __init__(self):
        super().__init__('dobot_gripper_cli')
        self.cli = self.create_client(GripperControl, 'dobot_gripper_service')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = GripperControl.Request()

    def send_request(self, gripper_state, keep_compressor_running):
        self.req.gripper_state = gripper_state
        self.req.keep_compressor_running = keep_compressor_running
        self.future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()


def main(args=None):
    rclpy.init(args=args)

    minimal_client = GripperClient()
    response = minimal_client.send_request("close", False)
    minimal_client.get_logger().info(
        'Result of calling service: %s' %(response))
    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()



dobot_demos/dobot_demos/gripper_client.py
jkaniuka/magician_ros2
from dobot_msgs.srv import ExecuteHomingProcedure
import rclpy
from rclpy.node import Node


class HomingClient(Node):

    def __init__(self):
        super().__init__('dobot_homing_cli')
        self.cli = self.create_client(ExecuteHomingProcedure, 'dobot_homing_service')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = ExecuteHomingProcedure.Request()


    def send_request(self):
        self.future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()


def main(args=None):
    rclpy.init(args=args)

    minimal_client = HomingClient()
    response = minimal_client.send_request()
    minimal_client.get_logger().info(response.instruction)
    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()



dobot_demos/dobot_demos/homing_client.py
jkaniuka/magician_ros2
import sys
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from action_msgs.msg import GoalStatus
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from dobot_msgs.action import PointToPoint
from dobot_msgs.srv import GripperControl


class PickAndPlace(Node):

    def __init__(self):
        super().__init__('pick_and_place_demo')
        self._action_client = ActionClient(self, PointToPoint, 'PTP_action', callback_group=ReentrantCallbackGroup())
        self.cli = self.create_client(srv_type = GripperControl, srv_name = 'dobot_gripper_service', callback_group = ReentrantCallbackGroup())
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = GripperControl.Request()

        self.tasks_list = [
            ["move", [125.0, -180.0, 60.0, 0.0], 1],
            ["gripper", "open", False],
            ["move", [125.0, -180.0, 30.0, 0.0], 1],
            ["gripper", "close", False],
            ["move", [125.0, -180.0, 60.0, 0.0], 1],
            ["move", [150.0, 190.0, 50.0, 0.0], 1],
            ["move", [150.0, 190.0, 20.0, 0.0], 1],
            ["gripper", "open", False],
            ["move", [150.0, 190.0, 50.0, 0.0], 1],
            ["gripper", "close", False],
            ["move", [200.0, 0.0, 100.0, 0.0], 1]
        ]

        self.goal_num = 0

    def execute(self):
            if self.goal_num > len(self.tasks_list)-1:
                rclpy.shutdown()
                sys.exit()
            else:
                self.get_logger().info('*** TASK NUM ***: {0}'.format(self.goal_num))

            if self.tasks_list[self.goal_num][0] == "gripper":
                self.send_request(*self.tasks_list[self.goal_num][1:])
                self.timer = self.create_timer(0.1, self.timer_callback, callback_group=ReentrantCallbackGroup())
                self.goal_num = self.goal_num + 1
            elif self.tasks_list[self.goal_num][0] == "move":
                self.send_goal(*self.tasks_list[self.goal_num][1:])
                self.goal_num = self.goal_num + 1


    def timer_callback(self):
        if self.srv_future.done():
            result = self.srv_future.result()
            self.get_logger().info('Result of service call: {0}'.format(result))
            self.timer.cancel()
            self.execute()

    def send_request(self, gripper_state, keep_compressor_running):
        self.req.gripper_state = gripper_state
        self.req.keep_compressor_running = keep_compressor_running
        self.srv_future = self.cli.call_async(self.req)


    def send_goal(self, _target, _type):
        goal_msg = PointToPoint.Goal()
        goal_msg.target_pose = _target
        goal_msg.motion_type = _type

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        status = future.result().status
        result = future.result().result
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Result of action call: {0}'.format(result))
            self.execute()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info('Received feedback: {0}'.format(feedback))


def main(args=None):
    rclpy.init(args=args)

    action_client = PickAndPlace()

    action_client.execute()

    executor = MultiThreadedExecutor()

    rclpy.spin(action_client, executor=executor)


if __name__ == '__main__':
    main()



dobot_demos/dobot_demos/pick_and_place.py
jkaniuka/magician_ros2
from dobot_msgs.srv import SuctionCupControl
import rclpy
from rclpy.node import Node


class SuctionCupClient(Node):

    def __init__(self):
        super().__init__('dobot_suction_cup_cli')
        self.cli = self.create_client(SuctionCupControl, 'dobot_suction_cup_service')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = SuctionCupControl.Request()

    def send_request(self, enable_suction):
        self.req.enable_suction = enable_suction
        self.future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()


def main(args=None):
    rclpy.init(args=args)

    minimal_client = SuctionCupClient()
    response = minimal_client.send_request(True)
    minimal_client.get_logger().info(
        'Result of calling service: %s' %(response))
    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()



dobot_demos/dobot_demos/suction_cup_client.py
jkaniuka/magician_ros2
from setuptools import setup

package_name = 'dobot_demos'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Sample scripts to test basic functionalities',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'test_gripper = dobot_demos.gripper_client:main',
            'test_homing = dobot_demos.homing_client:main',
            'test_point_to_point = dobot_demos.PTP_client:main',
            'test_suction_cup = dobot_demos.suction_cup_client:main',
            'test_pick_and_place = dobot_demos.pick_and_place:main',
        ],
    },
)


dobot_demos/setup.py
jkaniuka/magician_ros2
from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationEquals
from launch.substitutions import Command, LaunchConfiguration, PythonExpression

from launch_ros.actions import Node



def generate_launch_description():
    urdf_tutorial_path = get_package_share_path('dobot_description')
    default_model_path = urdf_tutorial_path / 'model/magician_standalone.urdf.xacro'
    default_rviz_config_path = urdf_tutorial_path / 'rviz/urdf_full.rviz'

    gui_arg = DeclareLaunchArgument(name='gui', default_value='false', choices=['true', 'false'],
                                    description='Flag to enable joint_state_publisher_gui.')

    model_arg = DeclareLaunchArgument(name='model', default_value=str(default_model_path),
                                      description='Absolute path to robot urdf file.')

    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
                                     description='Absolute path to rviz config file.')

    DOF_arg = DeclareLaunchArgument(name = 'DOF', default_value = '4', choices = ['3', '4'],
                                     description = 'Number of manipulator degrees of freedom.') 

    camera_arg = DeclareLaunchArgument(name = 'use_camera', default_value = 'false', choices = ['true', 'false'],
                                     description = 'Add Intel RealSense D435i camera to URDF model.')

    tool_arg = DeclareLaunchArgument(name = 'tool', default_value = 'gripper', 
                                     choices = ['suction_cup', 'gripper', 'extended_gripper', 'pen', 'none'],
                                     description = 'Type of tool.')    
                                  

    config_expression = ["('",LaunchConfiguration('DOF'),"'", " == '3'", 'and', \
                        "'",LaunchConfiguration('use_camera'),"'", " == 'false'", 'and (', \
                        "'",LaunchConfiguration('tool'),"'", " == 'none'", 'or', \
                        "'",LaunchConfiguration('tool'),"'", " == 'pen' ))", 'or', \
                        "('",LaunchConfiguration('DOF'),"'", " == '4'", 'and (', \
                        "'",LaunchConfiguration('tool'),"'", " == 'gripper'", 'or', \
                        "'",LaunchConfiguration('tool'),"'", " == 'extended_gripper'", 'or', \
                        "'",LaunchConfiguration('tool'),"'", " == 'suction_cup' ))" ]

    config_info = LogInfo(msg = '\n\n\n---------------------------------------------------------------------------\n '
'\n         AVAILABLE CONFIGURATIONS FOR DOBOT MAGICIAN:\n\n\n ' +
'If you cannot see the visualization, check if you entered the correct combination of parameters.\n\n ' +
'DOF:=3\n ' +
'   use_camera:=false\n ' +
'       tool:= none OR tool:=pen\n ' +
'DOF:=4\n ' +
'   use_camera:=true OR use_camera:=false\n ' +
'       tool:= gripper OR tool:=extended_gripper OR tool:=suction_cup\n\n' +
'If the real robot is disconnected and you want to see the visualization and be able to move the robot using joint_state_publisher_gui, add an gui:=true argument.\n\n\n' + 
'---------------------------------------------------------------------------')


    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model'),
                                            ' DOF:=', LaunchConfiguration('DOF'),
                                            ' use_camera:=', LaunchConfiguration('use_camera'),
                                            ' tool:=', LaunchConfiguration('tool')])}],
        condition = IfCondition(PythonExpression(config_expression)),
    )

    # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
    # joint_state_publisher_node = Node(
    #     package='joint_state_publisher',
    #     executable='joint_state_publisher',
    #     condition=UnlessCondition(LaunchConfiguration('gui'))
    # )

    joint_state_publisher_gui_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        condition=IfCondition(LaunchConfiguration('gui'))
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
        condition = IfCondition(PythonExpression(config_expression))
    )

    return LaunchDescription([
        gui_arg,
        model_arg,
        rviz_arg,
        DOF_arg,
        tool_arg,
        camera_arg,
        # joint_state_publisher_node,
        joint_state_publisher_gui_node,
        robot_state_publisher_node,
        rviz_node,
        config_info
    ])

dobot_description/launch/display.launch.py
jkaniuka/magician_ros2
from diagnostic_msgs.msg import DiagnosticArray
from dobot_msgs.msg import DobotAlarmCodes

import rclpy
from rclpy.clock import ROSClock
from rclpy.node import Node
from rclpy.qos import qos_profile_system_default

from dobot_diagnostics.diagnostics_utils import (
    joints_status,
    motors_status,
    sensors_status,
    links_status,
    MCU_status,
    FPGA_status,
    other_status
)

class DiagnosticTalker(Node):

    def __init__(self):
        super().__init__('dobot_diagnostic_talker')
        self.pub = self.create_publisher(DiagnosticArray,
                                         '/diagnostics',
                                         qos_profile_system_default)
        self.subscription = self.create_subscription(
            DobotAlarmCodes,
            'dobot_alarms',
            self.listener_callback,
            10)
        timer_period = 1.0
        self.tmr = self.create_timer(timer_period, self.timer_callback)
        self.alarms_list = []

        self.array = DiagnosticArray()
        self.array.status = []

    def listener_callback(self, msg):
        self.alarms_list = msg.alarms_list

    def timer_callback(self):
        self.array.header.stamp = ROSClock().now().to_msg()
        alalyzed_anarms = [] 
        alalyzed_anarms.extend(joints_status(self.alarms_list))
        alalyzed_anarms.extend(motors_status(self.alarms_list))
        alalyzed_anarms.extend(sensors_status(self.alarms_list))
        alalyzed_anarms.extend(links_status(self.alarms_list))
        alalyzed_anarms.extend(MCU_status(self.alarms_list))
        alalyzed_anarms.extend(FPGA_status(self.alarms_list))
        alalyzed_anarms.extend(other_status(self.alarms_list))

        self.array.status = alalyzed_anarms

        self.pub.publish(self.array)


def main(args=None):
    rclpy.init(args=args)

    node = DiagnosticTalker()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

dobot_diagnostics/dobot_diagnostics/alarms_parser.py
jkaniuka/magician_ros2
from diagnostic_msgs.msg import DiagnosticStatus, KeyValue

# ALARM CODES
# 0x00: 'reset occurred',
# 0x01: 'undefined instruction',
# 0x02: 'file system error',
# 0x03: 'communications error between MCU and FPGA',
# 0x04: 'angle sensor error',

# 0x10: 'plan: pose is abnormal',
# 0x11: 'plan: pose is out of workspace',
# 0x12: 'plan: joint limit',
# 0x13: 'plan: repetitive points',
# 0x14: 'plan: arc input parameter',
# 0x15: 'plan: jump parameter',

# 0x20: 'motion: kinematic singularity',
# 0x21: 'motion: out of workspace',
# 0x22: 'motion: inverse limit',

# 0x30: 'axis 1 overspeed',
# 0x31: 'axis 2 overspeed',
# 0x32: 'axis 3 overspeed',
# 0x33: 'axis 4 overspeed',

# 0x40: 'axis 1 positive limit',
# 0x41: 'axis 1 negative limit',
# 0x42: 'axis 2 positive limit',
# 0x43: 'axis 2 negative limit',
# 0x44: 'axis 3 positive limit',
# 0x45: 'axis 3 negative limit',
# 0x46: 'axis 4 positive limit',
# 0x47: 'axis 4 negative limit',
# 0x48: 'Parallelogram Positive Limitation Alarm',
# 0x49: 'Parallelogram Negative Limitation Alarm',

# 0x50: 'axis 1 lost steps',
# 0x51: 'axis 2 lost steps',
# 0x52: 'axis 3 lost steps',
# 0x53: 'axis 4 lost steps',

limited_position_description=[
    KeyValue(key='Reason', value="Joint position is close to the limits"),
    KeyValue(key="Solution", value="Move away from limits")]

overspeed_description=[
    KeyValue(key='Reason', value="The speed of motor is bigger than the max allowed value"),
    KeyValue(key="Solution", value="Decrease speed ratio to make joint speed smaller. Then reset alarm by using DoboControlPanel or calling ClearAlarms sevice")]

losing_step_description=[
    KeyValue(key='Reason', value="Dobot was hit during movement process or was trying to lift too heavy load"),
    KeyValue(key="Solution", value="Call ExecuteHomingProcedure service or use HOME button on DobotControlPanel")]

other_alarms_description=[
    KeyValue(key='Description', value="These alarms are software related, not hardware related, and are handled by another part of the control system. If such an alarm appeared, it is best to restart and re-power the robot and notify the maintainer of the package about it (by e-mail or via GitHub)."),
    KeyValue(key='Quick fix', value="Check alarm code in Dobot-Magician-ALARM-Description.pdf document and follow the recommendations")]


def joints_status(alarms_list):
    if 40 in alarms_list:
        diag_joint1 = warn_positive_limit('/joint1/Position')
    elif 41 in alarms_list:
        diag_joint1 = warn_negative_limit('/joint1/Position')
    else:
        diag_joint1 = ok_status('/joint1/Position')

    if 42 in alarms_list:
        diag_joint2 = warn_positive_limit('/joint2/Position')
    elif 43 in alarms_list:
        diag_joint2 = warn_negative_limit('/joint2/Position')
    else:
        diag_joint2 = ok_status('/joint2/Position')

    if 44 in alarms_list:
        diag_joint3 = warn_positive_limit('/joint3/Position')
    elif 45 in alarms_list:
        diag_joint3 = warn_negative_limit('/joint3/Position')
    else:
        diag_joint3 = ok_status('/joint3/Position')

    if 46 in alarms_list:
        diag_joint4 = warn_positive_limit('/joint4/Position')
    elif 47 in alarms_list:
        diag_joint4 = warn_negative_limit('/joint4/Position')
    else:
        diag_joint4 = ok_status('/joint4/Position')


    return [diag_joint1, diag_joint2, diag_joint3, diag_joint4]

def motors_status(alarms_list):

    # SPEED
    if 30 in alarms_list:
        diag_speed1 = warn_overspeed('/stepper1/Speed')
    else:
        diag_speed1 = ok_status('/stepper1/Speed')
    
    if 31 in alarms_list:
        diag_speed2 = warn_overspeed('/stepper2/Speed')
    else:
        diag_speed2 = ok_status('/stepper2/Speed')

    if 32 in alarms_list:
        diag_speed3 = warn_overspeed('/stepper3/Speed')
    else:
        diag_speed3 = ok_status('/stepper3/Speed')

    if 33 in alarms_list:
        diag_speed4 = warn_overspeed('/servo/Speed')
    else:
        diag_speed4 = ok_status('/servo/Speed')

    # LOST STEP DETECTION   
    if 50 in alarms_list:
        diag_lost_step1 = warn_lost_step('/stepper1/Losing-Step Detection')
    else:
        diag_lost_step1 = ok_status('/stepper1/Losing-Step Detection', "Running")
    
    if 51 in alarms_list:
        diag_lost_step2 = warn_lost_step('/stepper2/Losing-Step Detection')
    else:
        diag_lost_step2 = ok_status('/stepper2/Losing-Step Detection', "Running")

    if 52 in alarms_list:
        diag_lost_step3 = warn_lost_step('/stepper3/Losing-Step Detection')
    else:
        diag_lost_step3 = ok_status('/stepper3/Losing-Step Detection', "Running")

    if 53 in alarms_list:
        diag_lost_step4 = warn_lost_step('/servo/Losing-Step Detection')
    else:
        diag_lost_step4 = ok_status('/servo/Losing-Step Detection', "Running")


    return [diag_speed1, diag_lost_step1, diag_speed2, diag_lost_step2, diag_speed3, diag_lost_step3, diag_speed4, diag_lost_step4]

def sensors_status(alarms_list):
    if 4 not in alarms_list:
        diag_sensor = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name='/sensors/angle_sensor/Communication Status',
            message='OK')
    else:
        diag_sensor = DiagnosticStatus(
            level=DiagnosticStatus.ERROR,
            name='/sensors/angle_sensor/Communication Status',
            message='Error',
            values=[
                KeyValue(key='Reason', value="The angle sensor value can not be read correctly"),
                KeyValue(key="Solution", value="Re-power Dobot Magician, if the value is correct, that indicates reset alarm successfully")])

    return [diag_sensor]

def links_status(alarms_list):
    description=[
        KeyValue(key='Reason', value="Parallelogram is stretched to the limits"),
        KeyValue(key="Solution", value="Move away from limits")]

    if 48 in alarms_list:
        diag_link = DiagnosticStatus(
                    level=DiagnosticStatus.WARN,
                    name='/links/parallelogram/Position',
                    message='Positive limit violated',
                    values=description)
    elif 49 in alarms_list:
        diag_link = DiagnosticStatus(
                    level=DiagnosticStatus.WARN,
                    name='/links/parallelogram/Position',
                    message='Negative limit violated',
                    values=description)
    else:
         diag_link = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name='/links/parallelogram/Position',
            message='OK')

    return [diag_link]

def MCU_status(alarms_list):
    if 2 not in alarms_list:
        diag_mcu = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name='/computers/mcu/File System Status',
            message='OK')
    else:
        diag_mcu = DiagnosticStatus(
            level=DiagnosticStatus.ERROR,
            name='/computers/mcu/File System Status',
            message='Error',
            values=[
                KeyValue(key="Reason", value="Microcontroller file system error"),  
                KeyValue(key="Solution", value="Reset system using the Reset button on the back of the base. If file system is initialized successfully, the alarm is cleared automatically")])

    return [diag_mcu]

def FPGA_status(alarms_list):
    if 3 not in alarms_list:
        diag_fpga = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name='/computers/fpga/Communication with MCU Status',
            message='OK')
    else:
        diag_fpga = DiagnosticStatus(
            level=DiagnosticStatus.ERROR,
            name='/computers/fpga/Communication with MCU Status',
            message='Error',
            values=[
                KeyValue(key="Reason", value="There is a failed communication between MCU and FPGA when system is initializing"),
                KeyValue(key="Solution", value="Reset system using the Reset button on the back of the base. If the communication is successful, the alarm is cleared automatically")])

    return [diag_fpga]

def other_status(alarms_list):
    other_alarms_codes = [ 0, 1, 10, 11, 12, 13, 14, 15, 20, 21, 22]
    active_alarms = []
    for alarm in other_alarms_codes:
        if alarm in alarms_list:
            active_alarms.append(alarm)
    if not active_alarms:
        return [DiagnosticStatus(
                level = DiagnosticStatus.OK,
                name = '/other/Unhandled Exception',
                message = 'None')]
    else:
        status_list = "EXCEPTION CODES: " + str(active_alarms)[1:-1]
        return [DiagnosticStatus(
                level = DiagnosticStatus.ERROR,
                name = '/other/Unhandled Exception',
                message=status_list,
                values=other_alarms_description)]

# helpful functions (MACROS)

def ok_status(element, msg ='OK'):
    return DiagnosticStatus(
            level = DiagnosticStatus.OK,
            name = element,
            message = msg)

def warn_positive_limit(element):
    return DiagnosticStatus(
            level = DiagnosticStatus.WARN,
            name = element,
            message='Positive limit violated',
            values=limited_position_description)

def warn_negative_limit(element):
    return DiagnosticStatus(
            level = DiagnosticStatus.WARN,
            name = element,
            message='Negative limit violated',
            values=limited_position_description)

def warn_overspeed(element):
    return DiagnosticStatus(
            level = DiagnosticStatus.WARN,
            name = element,
            message='Overspeed',
            values=overspeed_description)

def warn_lost_step(element):
    return DiagnosticStatus(
            level = DiagnosticStatus.WARN,
            name = element,
            message='Missing steps',
            values=losing_step_description)
dobot_diagnostics/dobot_diagnostics/diagnostics_utils.py
jkaniuka/magician_ros2
"""Launch analyzer loader with parameters from yaml."""

import launch
import launch_ros.actions
from ament_index_python.packages import get_package_share_path



def generate_launch_description():

    diagnosticsc_package_path = get_package_share_path('dobot_diagnostics')
    analyzer_params_filepath = diagnosticsc_package_path / 'config/analyzer_params.yaml'


    aggregator = launch_ros.actions.Node(
        package='diagnostic_aggregator',
        executable='aggregator_node',
        output='screen',
        parameters=[analyzer_params_filepath])
    diag_publisher = launch_ros.actions.Node(
        package='dobot_diagnostics',
        executable='alarms_parser')
    return launch.LaunchDescription([
        aggregator,
        diag_publisher,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=aggregator,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )),
    ])

dobot_diagnostics/launch/alarms_analyzer.launch.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_diagnostics'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
        (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Diagnostics module for Dobot Magician',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'alarms_parser = dobot_diagnostics.alarms_parser:main',
        ],
    },
)

dobot_diagnostics/setup.py
jkaniuka/magician_ros2
from dobot_driver.interface import Interface
bot = Interface('/dev/ttyUSB0')
dobot_driver/dobot_driver/dobot_handle.py
jkaniuka/magician_ros2
import serial
import threading
import sys

from dobot_driver.message import Message

class Interface:
    def __init__(self, port):
        threading.Thread.__init__(self)
        self.lock = threading.Lock()

        try:
            self.serial = serial.Serial(
                port=port,
                baudrate=115200,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                bytesize=serial.EIGHTBITS
            )
        except:
            print("[WARN] Dobot Magician is not connected.")
            sys.exit(1)


    def send(self, message):
        self.lock.acquire()
        if self.serial.out_waiting != 0:
            self.serial.reset_output_buffer()
        self.serial.write(message.package())
        self.serial.flush()
        response = Message.read(self.serial)
        self.lock.release()
        if response is None:
            pass
        else: 
            return response.params

    def send_only(self, message):
        self.lock.acquire()
        if self.serial.out_waiting != 0:
            self.serial.reset_output_buffer()
        self.serial.write(message.package())
        self.serial.flush()
        self.lock.release()

    def connected(self):
        return self.serial.is_open

    def get_device_serial_number(self):
        request = Message([0xAA, 0xAA], 2, 0, False, False, [], direction='out')
        return self.send(request)

    def set_device_serial_number(self, serial_number):
        request = Message([0xAA, 0xAA], 2, 0, True, False, [serial_number], direction='out')
        return self.send(request)

    def get_device_name(self):
        request = Message([0xAA, 0xAA], 2, 1, False, False, [], direction='out')
        return self.send(request)

    def set_device_name(self, device_name):
        request = Message([0xAA, 0xAA], 2, 1, True, False, [device_name], direction='out')
        return self.send(request)

    def get_device_version(self):
        request = Message([0xAA, 0xAA], 2, 2, False, False, [], direction='out')
        return self.send(request)

    def set_sliding_rail_status(self, enable, version):
        request = Message([0xAA, 0xAA], 2, 3, True, False, [enable, version], direction='out')
        return self.send_only(request)

    # Time in milliseconds since start
    def get_device_time(self):
        request = Message([0xAA, 0xAA], 2, 4, False, False, [], direction='out')
        return self.send(request)

    def get_device_id(self):
        request = Message([0xAA, 0xAA], 2, 5, False, False, [], direction='out')
        return self.send(request)

    def get_pose(self):
        request = Message([0xAA, 0xAA], 2, 10, False, False, [], direction='out')
        return self.send(request)

    def reset_pose(self, manual, rear_arm_angle, front_arm_angle):
        request = Message([0xAA, 0xAA], 2, 11, True, False, [manual, rear_arm_angle, front_arm_angle], direction='out')
        return self.send(request)

    def get_sliding_rail_pose(self):
        request = Message([0xAA, 0xAA], 2, 13, False, False, [], direction='out')
        return self.send(request)

    def get_alarms_state(self):
        request = Message([0xAA, 0xAA], 2, 20, False, False, [], direction='out')
        return self.send(request)

    def clear_alarms_state(self):
        request = Message([0xAA, 0xAA], 2, 20, True, False, [], direction='out') #BUG
        return self.send(request)

    def get_homing_paramaters(self):
        request = Message([0xAA, 0xAA], 2, 30, False, False, [], direction='out')
        return self.send(request)

    def set_homing_parameters(self, x, y, z, r, queue=False): # changed from True
        request = Message([0xAA, 0xAA], 2, 30, True, queue, [x, y, z, r], direction='out')
        return self.send_only(request)

    def set_homing_command(self, command, queue=False): # changed from True
        request = Message([0xAA, 0xAA], 2, 31, True, queue, [command], direction='out')
        return self.send_only(request)

    # TODO: Reference is wrong here, arm does not send the said value
    def get_auto_leveling(self):
        request = Message([0xAA, 0xAA], 2, 32, False, False, [], direction='out')
        return self.send(request)

    def set_auto_leveling(self, enable, accuracy, queue=True):
        request = Message([0xAA, 0xAA], 2, 32, True, queue, [enable, accuracy], direction='out')
        return self.send(request)

    def get_handheld_teaching_mode(self):
        request = Message([0xAA, 0xAA], 2, 40, False, False, [], direction='out')
        return self.send(request)

    def set_handheld_teaching_mode(self, mode):
        request = Message([0xAA, 0xAA], 2, 40, True, False, [mode], direction='out')
        return self.send(request)

    def get_handheld_teaching_state(self):
        request = Message([0xAA, 0xAA], 2, 41, False, False, [], direction='out')
        return self.send(request)

    def set_handheld_teaching_state(self, enable):
        request = Message([0xAA, 0xAA], 2, 41, True, False, [enable], direction='out')
        return self.send(request)

    def get_handheld_teaching_trigger(self):
        request = Message([0xAA, 0xAA], 2, 42, False, False, [], direction='out')
        return self.send(request)

    def get_end_effector_params(self):
        request = Message([0xAA, 0xAA], 2, 60, False, False, [], direction='out')
        return self.send(request)

    def set_end_effector_params(self, bias_x, bias_y, bias_z):
        request = Message([0xAA, 0xAA], 2, 60, True, False, [bias_x, bias_y, bias_z], direction='out')
        return self.send_only(request)

    def get_end_effector_laser(self):
        request = Message([0xAA, 0xAA], 2, 61, False, False, [], direction='out')
        return self.send(request)

    def set_end_effector_laser(self, enable_control, enable_laser, queue=True):
        request = Message([0xAA, 0xAA], 2, 61, True, queue, [enable_control, enable_laser], direction='out')
        return self.send(request)

    def get_end_effector_suction_cup(self):
        request = Message([0xAA, 0xAA], 2, 62, False, False, [], direction='out')
        return self.send(request)

    def set_end_effector_suction_cup(self, enable_control, enable_suction, queue=False):
        request = Message([0xAA, 0xAA], 2, 62, True, queue, [enable_control, enable_suction], direction='out')
        return self.send_only(request)

    def get_end_effector_gripper(self):
        request = Message([0xAA, 0xAA], 2, 63, False, False, [], direction='out')
        return self.send(request)

    def set_end_effector_gripper(self, enable_control, enable_grip, queue=False):
        request = Message([0xAA, 0xAA], 2, 63, True, queue, [enable_control, enable_grip], direction='out')
        return self.send_only(request)

    def get_jog_joint_params(self):
        request = Message([0xAA, 0xAA], 2, 70, False, False, [], direction='out')
        return self.send(request)

    def set_jog_joint_params(self, velocity, acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 70, True, queue, velocity + acceleration, direction='out')
        return self.send_only(request)

    def get_jog_coordinate_params(self):
        request = Message([0xAA, 0xAA], 2, 71, False, False, [], direction='out')
        return self.send(request)

    def set_jog_coordinate_params(self, velocity, acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 71, True, queue, velocity + acceleration, direction='out')
        return self.send_only(request)

    def get_jog_common_params(self):
        request = Message([0xAA, 0xAA], 2, 72, False, False, [], direction='out')
        return self.send(request)

    def set_jog_common_params(self, velocity_ratio, acceleration_ratio, queue=False):
        request = Message([0xAA, 0xAA], 2, 72, True, queue, [velocity_ratio, acceleration_ratio], direction='out')
        return self.send_only(request)

    def set_jog_command(self, jog_type, command, queue=False):
        request = Message([0xAA, 0xAA], 2, 73, True, queue, [jog_type, command], direction='out')
        return self.send_only(request)

    def get_sliding_rail_jog_params(self):
        request = Message([0xAA, 0xAA], 2, 74, False, False, [], direction='out')
        return self.send(request)

    def set_sliding_rail_jog_params(self, velocity, acceleration, queue=True):
        request = Message([0xAA, 0xAA], 2, 74, True, queue, [velocity, acceleration], direction='out')
        return self.send(request)

    def get_point_to_point_joint_params(self):
        request = Message([0xAA, 0xAA], 2, 80, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_joint_params(self, velocity, acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 80, True, queue, velocity + acceleration, direction='out')
        return self.send_only(request)

    def get_point_to_point_coordinate_params(self):
        request = Message([0xAA, 0xAA], 2, 81, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_coordinate_params(self, coordinate_velocity, effector_velocity, coordinate_acceleration, effector_acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 81, True, queue, [coordinate_velocity, effector_velocity, coordinate_acceleration, effector_acceleration], direction='out')
        return self.send_only(request)

    def get_point_to_point_jump_params(self):
        request = Message([0xAA, 0xAA], 2, 82, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_jump_params(self, jump_height, z_limit, queue=True):
        request = Message([0xAA, 0xAA], 2, 82, True, queue, [jump_height, z_limit], direction='out')
        return self.send(request)

    def get_point_to_point_common_params(self):
        request = Message([0xAA, 0xAA], 2, 83, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_common_params(self, velocity_ratio, acceleration_ratio, queue=False):
        request = Message([0xAA, 0xAA], 2, 83, True, queue, [velocity_ratio, acceleration_ratio], direction='out')
        return self.send_only(request)

    def set_point_to_point_command(self, mode, x, y, z, r, queue=False): #changed from True
        request = Message([0xAA, 0xAA], 2, 84, True, queue, [mode, x, y, z, r], direction='out')
        return self.send_only(request)

    def get_point_to_point_sliding_rail_params(self):
        request = Message([0xAA, 0xAA], 2, 85, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_sliding_rail_params(self, velocity, acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 85, True, queue, [velocity, acceleration], direction='out')
        return self.send_only(request)

    def set_point_to_point_sliding_rail_command(self, mode, x, y, z, r, l, queue=False):
        request = Message([0xAA, 0xAA], 2, 86, True, queue, [mode, x, y, z, r, l], direction='out')
        return self.send_only(request)

    def get_point_to_point_jump2_params(self):
        request = Message([0xAA, 0xAA], 2, 87, False, False, [], direction='out')
        return self.send(request)

    def set_point_to_point_jump2_params(self, start_height, end_height, z_limit, queue=True):
        request = Message([0xAA, 0xAA], 2, 87, True, queue, [start_height, end_height, z_limit], direction='out')
        return self.send(request)

    # TODO: Reference is ambigious here - needs testing
    def set_point_to_point_po_command(self, mode, x, y, z, r, queue=True):
        request = Message([0xAA, 0xAA], 2, 88, True, queue, [mode, x, y, z, r], direction='out')
        return self.send(request)

    # TODO: Reference is ambigious here - needs testing
    def set_point_to_point_sliding_rail_po_command(self, ratio, address, level, queue=True):
        request = Message([0xAA, 0xAA], 2, 89, True, queue, [ratio, address, level], direction='out')
        return self.send(request)

    def get_continous_trajectory_params(self):
        request = Message([0xAA, 0xAA], 2, 90, False, False, [], direction='out')
        return self.send(request)

    def set_continous_trajectory_params(self, max_planned_acceleration, max_junction_velocity, acceleration, queue=False):
        request = Message([0xAA, 0xAA], 2, 90, True, queue, [max_planned_acceleration, max_junction_velocity, acceleration, 0], direction='out')
        return self.send_only(request)

    def set_continous_trajectory_real_time_params(self, max_planned_acceleration, max_junction_velocity, period, queue=True):
        request = Message([0xAA, 0xAA], 2, 90, True, queue, [max_planned_acceleration, max_junction_velocity, period, 1], direction='out')
        return self.send_only(request)

    def set_continous_trajectory_command(self, mode, x, y, z, velocity, queue=False):
        request = Message([0xAA, 0xAA], 2, 91, True, queue, [mode, x, y, z, velocity], direction='out')
        return self.send_only(request)

    def set_continous_trajectory_laser_engraver_command(self, mode, x, y, z, power, queue=True):
        request = Message([0xAA, 0xAA], 2, 92, True, queue, [mode, x, y, z, power], direction='out')
        return self.send(request)

    def get_arc_params(self):
        request = Message([0xAA, 0xAA], 2, 100, False, False, [], direction='out')
        return self.send(request)

    def set_arc_params(self, coordinate_velocity, effector_velocity, coordinate_acceleration, effector_acceleration, queue=True):
        request = Message([0xAA, 0xAA], 2, 100, True, queue, [coordinate_velocity, effector_velocity, coordinate_acceleration, effector_acceleration], direction='out')
        return self.send(request)

    def set_arc_command(self, circumference_point, ending_point, queue=True):
        request = Message([0xAA, 0xAA], 2, 101, True, queue, circumference_point + ending_point, direction='out')
        return self.send(request)

    def wait(self, milliseconds, queue=True):
        request = Message([0xAA, 0xAA], 2, 110, True, queue, [milliseconds], direction='out')
        return self.send(request)

    def set_trigger_command(self, address, mode, condition, threshold, queue=True):
        request = Message([0xAA, 0xAA], 2, 120, True, queue, [address, mode, condition, threshold], direction='out')
        return self.send(request)

    def get_io_multiplexing(self):
        request = Message([0xAA, 0xAA], 2, 130, False, False, [], direction='out')
        return self.send(request)

    def set_io_multiplexing(self, address, multiplex, queue=True):
        request = Message([0xAA, 0xAA], 2, 130, True, queue, [address, multiplex], direction='out')
        return self.send(request)

    def get_io_do(self):
        request = Message([0xAA, 0xAA], 2, 131, False, False, [], direction='out')
        return self.send(request)

    def set_io_do(self, address, level, queue=True):
        request = Message([0xAA, 0xAA], 2, 131, True, queue, [address, level], direction='out')
        return self.send(request)

    def get_io_pwm(self):
        request = Message([0xAA, 0xAA], 2, 132, False, False, [], direction='out')
        return self.send(request)

    def set_io_pwm(self, address, frequency, duty_cycle, queue=True):
        request = Message([0xAA, 0xAA], 2, 132, True, queue, [address, frequency, duty_cycle], direction='out')
        return self.send(request)

    def get_io_di(self):
        request = Message([0xAA, 0xAA], 2, 133, False, False, [], direction='out')
        return self.send(request)

    def get_io_adc(self):
        request = Message([0xAA, 0xAA], 2, 134, False, False, [], direction='out')
        return self.send(request)

    def set_extended_motor_velocity(self, index, enable, speed, queue=True):
        request = Message([0xAA, 0xAA], 2, 135, True, queue, [index, enable, speed], direction='out')
        return self.send(request)

    def get_color_sensor(self, index):
        request = Message([0xAA, 0xAA], 2, 137, False, False, [], direction='out')
        return self.send(request)

    def set_color_sensor(self, index, enable, port, version, queue=True):
        request = Message([0xAA, 0xAA], 2, 137, True, queue, [enable, port, version], direction='out')
        return self.send(request)

    def get_ir_switch(self, index):
        request = Message([0xAA, 0xAA], 2, 138, False, False, [], direction='out')
        return self.send(request)

    def set_ir_switch(self, index, enable, port, version, queue=True):
        request = Message([0xAA, 0xAA], 2, 138, True, queue, [enable, port, version], direction='out')
        return self.send(request)

    def get_angle_sensor_static_error(self, index):
        request = Message([0xAA, 0xAA], 2, 140, False, False, [], direction='out')
        return self.send(request)

    def set_angle_sensor_static_error(self, index, rear_arm_angle_error, front_arm_angle_error):
        request = Message([0xAA, 0xAA], 2, 140, True, False, [rear_arm_angle_error, front_arm_angle_error], direction='out')
        return self.send(request)

    def get_wifi_status(self):
        request = Message([0xAA, 0xAA], 2, 150, False, False, [], direction='out')
        return self.send(request)

    def set_wifi_status(self, index, enable):
        request = Message([0xAA, 0xAA], 2, 150, True, False, [enable], direction='out')
        return self.send(request)

    def get_wifi_ssid(self):
        request = Message([0xAA, 0xAA], 2, 151, False, False, [], direction='out')
        return self.send(request)

    def set_wifi_ssid(self, index, ssid):
        request = Message([0xAA, 0xAA], 2, 151, True, False, [ssid], direction='out')
        return self.send(request)

    def get_wifi_password(self):
        request = Message([0xAA, 0xAA], 2, 152, False, False, [], direction='out')
        return self.send(request)

    def set_wifi_password(self, index, ssid):
        request = Message([0xAA, 0xAA], 2, 152, True, False, [ssid], direction='out')
        return self.send(request)

    def get_wifi_address(self):
        request = Message([0xAA, 0xAA], 2, 153, False, False, [], direction='out')
        return self.send(request)

    # 192.168.1.1 = a.b.c.d
    def set_wifi_address(self, index, use_dhcp, a, b, c, d):
        request = Message([0xAA, 0xAA], 2, 153, True, False, [use_dhcp, a, b, c, d], direction='out')
        return self.send(request)

    def get_wifi_netmask(self):
        request = Message([0xAA, 0xAA], 2, 154, False, False, [], direction='out')
        return self.send(request)

    # 255.255.255.0 = a.b.c.d
    def set_wifi_netmask(self, index, a, b, c, d):
        request = Message([0xAA, 0xAA], 2, 154, True, False, [a, b, c, d], direction='out')
        return self.send(request)

    def get_wifi_gateway(self):
        request = Message([0xAA, 0xAA], 2, 155, False, False, [], direction='out')
        return self.send(request)

    # 192.168.1.1 = a.b.c.d
    def set_wifi_gateway(self, index, use_dhcp, a, b, c, d):
        request = Message([0xAA, 0xAA], 2, 155, True, False, [use_dhcp, a, b, c, d], direction='out')
        return self.send(request)

    def get_wifi_dns(self):
        request = Message([0xAA, 0xAA], 2, 156, False, False, [], direction='out')
        return self.send(request)

    # 192.168.1.1 = a.b.c.d
    def set_wifi_dns(self, index, use_dhcp, a, b, c, d):
        request = Message([0xAA, 0xAA], 2, 156, True, False, [use_dhcp, a, b, c, d], direction='out')
        return self.send(request)

    def get_wifi_connect_status(self):
        request = Message([0xAA, 0xAA], 2, 157, False, False, [], direction='out')
        return self.send(request)

    def set_lost_step_params(self, param):
        request = Message([0xAA, 0xAA], 2, 170, True, False, [param], direction='out')
        return self.send(request)

    def set_lost_step_command(self):
        request = Message([0xAA, 0xAA], 2, 171, True, False, [], direction='out')
        return self.send(request)

    def start_queue(self):
        request = Message([0xAA, 0xAA], 2, 240, True, False, [], direction='out')
        return self.send_only(request)

    def stop_queue(self, force=False):
        request = Message([0xAA, 0xAA], 2, 242 if force else 241, True, False, [], direction='out')
        return self.send_only(request)

    def start_queue_download(self, total_loop, line_per_loop):
        request = Message([0xAA, 0xAA], 2, 243, True, False, [total_loop, line_per_loop], direction='out')
        return self.send(request)

    def stop_queue_download(self):
        request = Message([0xAA, 0xAA], 2, 244, True, False, [], direction='out')
        return self.send(request)

    def clear_queue(self):
        request = Message([0xAA, 0xAA], 2, 245, True, False, [], direction='out')
        return self.send_only(request)

    def get_current_queue_index(self):
        request = Message([0xAA, 0xAA], 2, 246, True, False, [], direction='out')
        return self.send(request)


def print_msg(request):
    print(request.header)
    print(request.length)
    print(request.id)
    print(request.rw)
    print(request.is_queued)
    print(request.raw_params)
    print(request.params)
    print(request.package())
dobot_driver/dobot_driver/interface.py
jkaniuka/magician_ros2
import sys
from dobot_driver.parsers import parsers

class Message:
    def __init__(self, header, length, id, rw, is_queued, params, direction='in'):
        self.header = header
        self.length = length
        self.id = id
        self.rw = rw
        self.is_queued = is_queued
        self.raw_params = []
        self.params = []

        if direction == 'in':
            self.raw_params = params
            self.params = self.parse_params('in')
        elif direction == 'out':
            self.params = params
            self.raw_params = self.parse_params('out')

    @staticmethod
    def calculate_checksum(payload):
        r = sum(payload) % 256
        # Calculate the two's complement
        check_byte = (256 - r) % 256
        return check_byte

    @staticmethod
    def verify_checksum(payload, checksum):
        a = sum(payload) % 256
        is_correct = True if (a + checksum) % 256 == 0 else False
        return is_correct

    @staticmethod
    def parse(message):
        bytes = list(message)

        header = bytes[0:2]
        length = bytes[2]
        id = bytes[3]
        control = bytes[4]
        rw = (control & 1) == 1
        is_queued = ((control & 2) >> 1) == 1
        params = bytes[5:-1]
        checksum = bytes[-1]

        if id==10 or id==20 or id==13:
            verified = Message.verify_checksum([id] + [control] + params, checksum)

            if verified:
                return Message(header, length, id, rw, is_queued, params)
            else:
                return None
        else:
            return None

    @staticmethod
    def read(serial):
        header = serial.read(2)
        if header != b'\xaa\xaa':
            return None
        length = int.from_bytes(serial.read(1), 'little')
        payload = serial.read(length)
        checksum = serial.read(1)

        return Message.parse(header + bytes([length]) + payload + checksum)

    def parse_params(self, direction):
        message_parsers = parsers[self.id]

        if direction == 'in':
            if message_parsers is None:
                return None

            parser = None
            if self.rw == 0 and self.is_queued == 0:
                parser = message_parsers[0]
            elif self.rw == 1 and self.is_queued == 0:
                parser = message_parsers[0]
            elif self.rw == 1 and self.is_queued == 1:
                parser = message_parsers[2]

            if parser is None:
                return []

            return parser(self.raw_params)
        elif direction == 'out':
            if message_parsers is None:
                return []

            parser = None
            if direction == 'out' and self.rw == 1:
                parser = message_parsers[3]

            if parser is None:
                return []

            return parser(self.params)

    def package(self):
        self.length = 2 + len(self.raw_params)
        control = int('000000' + str(int(self.is_queued)) + str(int(self.rw)), 2)
        self.checksum = Message.calculate_checksum([self.id] + [control] + self.raw_params)

        result = bytes(self.header + [self.length] + [self.id] + [control] + self.raw_params + [self.checksum])

        return result

dobot_driver/dobot_driver/message.py
jkaniuka/magician_ros2
import struct

# This file contains each parser for the communication specification v1.1.5
# Note that long exposure to the code may and likely will cause a quick and sudden death
# It is advised that you do not put yourself to risk and you should therefore close this
# file as soon as possible

# The rationale around implementing the parser this way is due to it being concise,
# somewhat readable (given you have access to the API reference) and scalable -
# with the ability to easily add or change parsers in the future

# Format:
# key = message id according to API reference
# value[0] = parser for response to getters (direction=in, rw=0, isQueued=0)
# value[1] = parser for response to setters (direction=in, rw=1, isQueued=0)
# value[2] = parser for response to setters (direction=in, rw=1, isQueued=1)
# value[3] = parser for request to setters (direction=out, rw=1, isQueued=0/1)

parsers = {
    # Device information
    0: [lambda x: ''.join(map(chr, x)), None, None, lambda x: list(x[0].encode('ascii')) + [0x00]],
    1: [lambda x: ''.join(map(chr, x)), None, None, lambda x: list(x[0].encode('ascii')) + [0x00]],
    2: [lambda x: x[0:3], None, None, None],
    3: [lambda x: struct.unpack('<B', bytearray(x))[0], None, None, lambda x: list(struct.pack('<BB', *x))],
    4: [lambda x: struct.unpack('<L', bytearray(x))[0], None, None, None],
    5: [lambda x: struct.unpack('<' + 'L' * 3, bytearray(x)), None, None, None],
    # Real-time pose
    10: [lambda x: struct.unpack('<' + 'f' * 8, bytearray(x)), None, None, None],
    11: [None, None, None, lambda x: list(struct.pack('<Bff', *x))],
    13: [lambda x: struct.unpack('<f', bytearray(x))[0], None, None, None],
    # Alarm
    20: [lambda x: struct.unpack('<' + 'B' * 16, bytearray(x)), None, None, None], #BUG
    21: [None, None, None, None],
    # Homing function
    30: [lambda x: struct.unpack('<' + 'f' * 4, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 4, *x))],
    31: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<f', *x))],
    32: [lambda x: struct.unpack('<Bf', bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bf', *x))],
    # Handhold teaching
    40: [lambda x: x[0], None, None, lambda x: list(struct.pack('<B', *x))],
    41: [lambda x: x[0] == 1, None, None, lambda x: list(struct.pack('<B', *x))],
    42: [lambda x: x[0] == 1, None, None, None],
    # End effector
    60: [None, None, None, lambda x: list(struct.pack('<' + 'f' * 3, *x))], #BUG in original lib version was 4 instead of 3 !
    61: [lambda x: (x[0] == 1, x[1] == 2), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<BB', *x))],
    62: [lambda x: (x[0] == 1, x[1] == 2), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<BB', *x))],
    63: [lambda x: (x[0] == 1, x[1] == 2), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<BB', *x))],
    # JOG
    70: [lambda x: struct.unpack('<' + 'f' * 8, bytearray(x)), None, None, lambda x: list(struct.pack('<' + 'f' * 8, *x))],
    71: [lambda x: struct.unpack('<' + 'f' * 8, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 8, *x))],
    72: [lambda x: struct.unpack('<' + 'f' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    73: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'B' * 2, *x))],
    74: [lambda x: struct.unpack('<' + 'f' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    # PTP
    80: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 8, *x))],
    81: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 4, *x))],
    82: [lambda x: struct.unpack('<' + 'f' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    83: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    84: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bffff', *x))],
    85: [lambda x: struct.unpack('<' + 'f' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    86: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bfffff', *x))],
    87: [lambda x: struct.unpack('<' + 'f' * 3, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 3, *x))],
    88: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bffff', *x))],
    89: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bfffff', *x))],
    # Continuous path
    90: [lambda x: struct.unpack('<fffB', bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<fffB', *x))],
    91: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bffff', *x))],
    92: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bffff', *x))],
    # Arc
    100: [lambda x: struct.unpack('<' + 'f' * 4, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 4, *x))],
    101: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'f' * 8, *x))],
    # Wait
    110: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<L', *x))],
    # Triggers
    120: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<BBBH', *x))],
    # EIO
    130: [lambda x: struct.unpack('<B' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'B' * 2, *x))],
    131: [lambda x: struct.unpack('<B' * 2, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'B' * 2, *x))],
    132: [lambda x: struct.unpack('<Bff', bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<Bff', *x))],
    133: [lambda x: struct.unpack('<B' * 2, bytearray(x)), None, None, None],
    134: [lambda x: struct.unpack('<BH', bytearray(x)), None, None, None],
    135: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<BBf', *x))],
    137: [lambda x: struct.unpack('<B' * 3, bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'B' * 3, *x))],
    138: [lambda x: struct.unpack('<B', bytearray(x)), None, lambda x: struct.unpack('<Q', bytearray(x))[0], lambda x: list(struct.pack('<' + 'B' * 3, *x))],
    # Calibration
    140: [lambda x: struct.unpack('<' + 'f' * 2, bytearray(x)), None, None, lambda x: list(struct.pack('<' + 'f' * 2, *x))],
    # Wifi
    150: [lambda x: x[0] == 1, None, None, lambda x: list(struct.pack('<B', *x))],
    151: [lambda x: ''.join(map(chr, x)), None, None, lambda x: list(x[0].encode('ascii')) + [0x00]],
    152: [lambda x: ''.join(map(chr, x)), None, None, lambda x: list(x[0].encode('ascii')) + [0x00]],
    153: [lambda x: struct.unpack('<B' * 5, bytearray(x)), None, None, lambda x: list(struct.pack('<B' * 5, *x))],
    154: [lambda x: struct.unpack('<B' * 4, bytearray(x)), None, None, lambda x: list(struct.pack('<B' * 4, *x))],
    155: [lambda x: struct.unpack('<B' * 4, bytearray(x)), None, None, lambda x: list(struct.pack('<B' * 4, *x))],
    156: [lambda x: struct.unpack('<B' * 4, bytearray(x)), None, None, lambda x: list(struct.pack('<B' * 4, *x))],
    157: [lambda x: x[0] == 1, None, None, None],
    # Losing-step detection
    170: [None, None, None, lambda x: list(struct.pack('<f', *x))],
    171: [None, None, lambda x: struct.unpack('<Q', bytearray(x))[0], None],
    # Queued execution control
    240: [None, None, None, None],
    241: [None, None, None, None],
    242: [None, None, None, None],
    243: [None, None, None, lambda x: list(struct.pack('<L' * 2, *x))],
    244: [None, None, None, None],
    245: [None, None, None, None],
    246: [lambda x: struct.unpack('<Q', bytearray(x))[0], None, None, None]
}

dobot_driver/dobot_driver/parsers.py
jkaniuka/magician_ros2
from dobot_msgs.srv import GripperControl
from dobot_msgs.msg import GripperStatus
import rclpy
from rclpy.node import Node
from dobot_driver.dobot_handle import bot
import time


class GripperService(Node):

    def __init__(self):
        super().__init__('dobot_gripper_srv')
        self.srv = self.create_service(GripperControl, 'dobot_gripper_service', self.gripper_callback)
        self.publisher_ = self.create_publisher(GripperStatus, 'gripper_status_rviz', 10)
        self.gripper_latency = 0.500


    def gripper_callback(self, request, response):

        msg = GripperStatus()
        msg.header.stamp = self.get_clock().now().to_msg()

        if request.gripper_state == "open" and request.keep_compressor_running == True:
            bot.set_end_effector_gripper(True, False)
            time.sleep(self.gripper_latency)
            msg.status = "opened"
            self.publisher_.publish(msg)
        elif request.gripper_state == "open" and request.keep_compressor_running == False:
            bot.set_end_effector_gripper(True, False)
            time.sleep(self.gripper_latency)
            bot.set_end_effector_gripper(False, False)
            msg.status = "opened"
            self.publisher_.publish(msg)
        elif request.gripper_state == "close" and request.keep_compressor_running == True:
            bot.set_end_effector_gripper(True, True)
            time.sleep(self.gripper_latency)
            msg.status = "closed"
            self.publisher_.publish(msg)
        elif request.gripper_state == "close" and request.keep_compressor_running == False:
            bot.set_end_effector_gripper(True, True)
            time.sleep(self.gripper_latency)
            bot.set_end_effector_gripper(False, True)
            msg.status = "closed"
            self.publisher_.publish(msg)
        else:
            response.success = False
            response.message = "Invalid service request"
            return response

        response.success = True
        response.message = "Gripper state has been changed"
        return response




def main(args=None):
    rclpy.init(args=args)

    minimal_service = GripperService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_end_effector/dobot_end_effector/gripper_server.py
jkaniuka/magician_ros2
from dobot_msgs.srv import SuctionCupControl
import rclpy
from rclpy.node import Node
from dobot_driver.dobot_handle import bot
import time


class SuctionCupService(Node):

    def __init__(self):
        super().__init__('dobot_suction_cup_srv')
        self.srv = self.create_service(SuctionCupControl, 'dobot_suction_cup_service', self.suction_cup_callback)
        self.suction_cup_latency = 0.500


    def suction_cup_callback(self, request, response):


        if request.enable_suction == True:
            bot.set_end_effector_suction_cup(True, True)
            time.sleep(self.suction_cup_latency)
 
        elif request.enable_suction == False:
            bot.set_end_effector_suction_cup(False, False)
            time.sleep(self.suction_cup_latency)

        else:
            response.success = False
            response.message = "Invalid service request"
            return response

        response.success = True
        response.message = "Suction cup state has been changed"
        return response




def main(args=None):
    rclpy.init(args=args)

    minimal_service = SuctionCupService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_end_effector/dobot_end_effector/suction_cup_server.py
jkaniuka/magician_ros2
from setuptools import setup

package_name = 'dobot_end_effector'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='ROS 2 service-based interfaces to operate Dobot Magician end-effectors.',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'gripper_server = dobot_end_effector.gripper_server:main',
            'suction_cup_server = dobot_end_effector.suction_cup_server:main',
        ],
    },
)

dobot_end_effector/setup.py
jkaniuka/magician_ros2
from dobot_msgs.srv import ExecuteHomingProcedure
import rclpy
from rclpy.node import Node
from dobot_driver.dobot_handle import bot
import time
from rcl_interfaces.msg import SetParametersResult, ParameterDescriptor
from rclpy.parameter import Parameter



class HomingService(Node):

    def __init__(self):
        super().__init__('dobot_homing_srv')
        self.srv = self.create_service(ExecuteHomingProcedure, 'dobot_homing_service', self.homing_callback)
        self.instruction = "The homing procedure has started. Wait until the arm stops moving and the led stops flashing blue and turns green."
        self.parameter_descriptor = ParameterDescriptor(description='Position where Dobot finishes homing procedure expressed in Cartesian coordinates x, y, z, r.', additional_constraints = "Target position must be within Dobot's range! The unit is millimeters.")
        self.declare_parameter('homing_position', rclpy.Parameter.Type.DOUBLE_ARRAY, self.parameter_descriptor)

        self.add_on_set_parameters_callback(self.parameters_callback)


    def parameters_callback(self, params):

        for param in params:
            if param.name == 'homing_position' and param.type_ == Parameter.Type.DOUBLE_ARRAY:
                bot.set_homing_parameters(param.value[0], param.value[1], param.value[2], param.value[3])
                return SetParametersResult(successful=True)
            else:
                return SetParametersResult(successful=False)



    def homing_callback(self, request, response):

        bot.set_homing_command(0) 
        time.sleep(0.1)

        response.success = True
        response.instruction = self.instruction
        return response




def main(args=None):
    rclpy.init(args=args)

    minimal_service = HomingService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_homing/dobot_homing/homing_server.py
jkaniuka/magician_ros2
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    config = os.path.join(
        get_package_share_directory('dobot_homing'),
        'config',
        'homing_params.yaml'
        )

    return LaunchDescription([
        Node(
            package='dobot_homing',
            executable='homing_server',
            output='screen',
        ),
    ExecuteProcess(
        cmd=[[
            'sleep 5;', 'ros2 ' , 'param ',  'load ', '/dobot_homing_srv ', config
        ]],
        shell=True
    )
    ])


dobot_homing/launch/dobot_homing.launch.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_homing'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Dobot Magician homing procedure service.',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'homing_server = dobot_homing.homing_server:main',
        ],
    },
)

dobot_homing/setup.py
jkaniuka/magician_ros2
import time
import numpy as np
import pybullet as pyb
import pybullet_data
import os.path as path
import sys
from math import radians
from dobot_kinematics.collision_utils import NamedCollisionObject, CollisionDetector
import math
from dobot_kinematics.dobot_inv_kin import calc_inv_kin
import os



global model_config


class PyBulletCollisionServer():


    @staticmethod
    def load_environment(client_id):

        pyb.setAdditionalSearchPath(
            pybullet_data.getDataPath(), physicsClientId=client_id
        )

        model_prefix = path.abspath(path.join(__file__ ,"../../..")) 
        model_prefix = model_prefix.split("dobot_kinematics",1)[0]+ "dobot_description/share/dobot_description/meshes/collision/urdf_models/"

        tool_mounted = os.environ.get('MAGICIAN_TOOL')

        if tool_mounted == "none":
            path_to_collision_model = model_prefix + "3DOF.urdf"
        elif tool_mounted == "pen":
            path_to_collision_model = model_prefix + "pen.urdf"
        elif tool_mounted == "suction_cup":
            path_to_collision_model = model_prefix + "suction_cup.urdf"
        elif tool_mounted == "gripper":
            path_to_collision_model = model_prefix + "gripper.urdf"
        elif tool_mounted == "extended_gripper":
            path_to_collision_model = model_prefix + "extended_gripper.urdf"


        global model_config
        prefix, model_config = os.path.split(path_to_collision_model)

        dobot_magician_id = pyb.loadURDF(
            path_to_collision_model,
            [0, 0, 0],
            useFixedBase=True,
            physicsClientId=client_id,
        )

        ground_id = pyb.loadURDF(
            "plane.urdf", [0, 0, -0.131305], useFixedBase=True, physicsClientId=client_id
        )

        bodies = {
            "robot": dobot_magician_id,
            "ground": ground_id,
        }

        for id in bodies.values():
            if id <= -1 :
                print("Could not load URDF model")
                sys.exit(1)

        return bodies


    @staticmethod
    def set_robot_configuration(angles):
        """Set robot configuration."""
        q = np.zeros(4)
        q[0] = radians(angles[0])
        q[1] = radians(angles[1])
        q[2] = radians(angles[2]) - q[1]
        q[3] = -(q[1]+q[2])
        return q

    @staticmethod
    def update_robot_configuration(robot_id, q, gui_id):
        """Set the robot configuration."""
        for i in range(4):
            pyb.resetJointState(
                robot_id, i+1, q[i], physicsClientId = gui_id
            )


    @staticmethod
    def linear_trajecory_to_discrete_waypoints(start, target, step_len = 0.5):
        waypoints = []

        x, y, z = [start[0], target[0]], [start[1], target[1]], [start[2], target[2]]

        steps_num = 1
        while True:
            dist_now = math.dist([x[0], y[0], z[0]], [x[0] + (x[1]-x[0])*(1/steps_num), y[0] + (y[1]-y[0])*(1/steps_num), z[0] + (z[1]-z[0])*(1/steps_num)])
            dist_next = math.dist([x[0], y[0], z[0]], [x[0] + (x[1]-x[0])*(1/(steps_num+1)), y[0] + (y[1]-y[0])*(1/(steps_num+1)), z[0] + (z[1]-z[0])*(1/(steps_num+1))])

            if dist_now > step_len and dist_next < step_len:
                steps_num = steps_num + 3
                break
            else:
                steps_num = steps_num + 1

        for t in range(steps_num-1):
            t =t / (steps_num-2)
            waypoints.append([x[0] + (x[1]-x[0])*t, y[0] + (y[1]-y[0])*t, z[0] + (z[1]-z[0])*t])

        return waypoints

    
    def validate_trajectory(self, motion_type, current_pose, target_point, detect_ground):
        # MOTION_TYPE_MOVJ_XYZ 1
        # MOTION_TYPE_MOVL_XYZ 2
        # MOTION_TYPE_MOVJ_ANGLE 4
        # MOTION_TYPE_MOVL_ANGLE 5
        col_id = pyb.connect(pyb.DIRECT, options="--opengl3")

        if col_id == -1:
            print("Not connected")
            sys.exit(1)


        collision_bodies = PyBulletCollisionServer.load_environment(col_id)

        robot_id = collision_bodies["robot"]

        end_effector_link = NamedCollisionObject("robot", "magician_link_4") 
        if model_config != "3DOF.urdf":
            gripper = NamedCollisionObject("robot", "end_effector_part")  
        base = NamedCollisionObject("robot", "magician_base_link")
        link1 = NamedCollisionObject("robot", "magician_link_1")
        ground = NamedCollisionObject("ground")


        if detect_ground == True and model_config=="3DOF.urdf":
            col_detector = CollisionDetector(
                col_id,
                collision_bodies,
                [(end_effector_link, base), (end_effector_link, ground), (end_effector_link, link1)],
            )
        elif detect_ground == False and model_config=="3DOF.urdf":
            col_detector = CollisionDetector(
                col_id,
                collision_bodies,
                [(end_effector_link, base), (end_effector_link, link1)],
            )
        elif detect_ground == True and model_config!="3DOF.urdf":
            col_detector = CollisionDetector(
                col_id,
                collision_bodies,
                [(end_effector_link, base), (end_effector_link, ground), (end_effector_link, link1), (gripper, base), (gripper, ground), (gripper, link1)],
            )
        elif detect_ground == False and model_config!="3DOF.urdf":
            col_detector = CollisionDetector(
                col_id,
                collision_bodies,
                [(end_effector_link, base), (end_effector_link, link1), (gripper, base), (gripper, link1)],
            )
        

        if motion_type == 1 or motion_type == 4:
            if motion_type == 1:
                angles = calc_inv_kin(*target_point)
                q = PyBulletCollisionServer.set_robot_configuration(angles[0:3])
            elif motion_type == 4:
                q = PyBulletCollisionServer.set_robot_configuration(target_point)
            PyBulletCollisionServer.update_robot_configuration(robot_id, q, gui_id=col_id)
            pyb.stepSimulation(physicsClientId=col_id)

            in_col = col_detector.in_collision(
                q, margin = 0.0
            )
            if in_col:
                return False

            return True

        if motion_type == 2 or motion_type == 5:
            waypoints = PyBulletCollisionServer.linear_trajecory_to_discrete_waypoints(current_pose, target_point)
            for point in waypoints:
                if motion_type == 2:
                    end_tool_rotation = target_point.tolist()[3]
                elif motion_type == 5:
                    end_tool_rotation = target_point[3]
                point.append(end_tool_rotation)
                angles = calc_inv_kin(*point)
                q = PyBulletCollisionServer.set_robot_configuration(angles[0:3])
                PyBulletCollisionServer.update_robot_configuration(robot_id, q, gui_id=col_id)
                pyb.stepSimulation(physicsClientId=col_id)
                
                in_col = col_detector.in_collision(
                    q, margin = 0.0
                )
                if in_col:
                    return False


            return True

dobot_kinematics/dobot_kinematics/collision_detection_server.py
jkaniuka/magician_ros2
from dataclasses import dataclass

import numpy as np
import pybullet as pyb


@dataclass
class NamedCollisionObject:
    """Name of a body and one of its links.

    The body name must correspond to the key in the `bodies` dict, but is
    otherwise arbitrary. The link name should match the URDF. The link name may
    also be None, in which case the base link (index -1) is used.
    """

    body_name: str
    link_name: str = None


@dataclass
class IndexedCollisionObject:
    """Index of a body and one of its links."""

    body_uid: int
    link_uid: int


def index_collision_pairs(physics_uid, bodies, named_collision_pairs):
    """Convert a list of named collision pairs to indexed collision pairs.

    In other words, convert named bodies and links to the indexes used by
    PyBullet to facilate computing collisions between the objects.

    Parameters:
      physics_uid: Index of the PyBullet physics server to use.
      bodies: dict with body name keys and corresponding indices as values
      named_collision_pairs: a list of 2-tuples of NamedCollisionObject

    Returns: a list of 2-tuples of IndexedCollisionObject
    """

    # build a nested dictionary mapping body names to link names to link
    # indices
    body_link_map = {}
    for name, uid in bodies.items():
        body_link_map[name] = {}
        n = pyb.getNumJoints(uid, physics_uid)
        for i in range(n):
            info = pyb.getJointInfo(uid, i, physics_uid)
            link_name = info[12].decode("utf-8")
            body_link_map[name][link_name] = i

    def _index_named_collision_object(obj):
        """Map body and link names to corresponding indices."""
        body_uid = bodies[obj.body_name]
        if obj.link_name is not None:
            link_uid = body_link_map[obj.body_name][obj.link_name]
        else:
            link_uid = -1
        return IndexedCollisionObject(body_uid, link_uid)

    # convert all pairs of named collision objects to indices
    indexed_collision_pairs = []
    for a, b in named_collision_pairs:
        a_indexed = _index_named_collision_object(a)
        b_indexed = _index_named_collision_object(b)
        indexed_collision_pairs.append((a_indexed, b_indexed))


    return indexed_collision_pairs


class CollisionDetector:
    def __init__(self, col_id, bodies, named_collision_pairs):
        self.col_id = col_id
        self.bodies = bodies
        self.robot_id = bodies["robot"]
        self.indexed_collision_pairs = index_collision_pairs(
            self.col_id, bodies, named_collision_pairs
        )

    def compute_distances(self, q, max_distance=1.0):
        """Compute closest distances for a given configuration.

        Parameters:
          q: Iterable representing the desired configuration. This is applied
             directly to PyBullet body with index bodies["robot"].
          max_distance: Bodies farther apart than this distance are not queried
             by PyBullet, the return value for the distance between such bodies
             will be max_distance.

        Returns: A NumPy array of distances, one per pair of collision objects.
        """

        # put the robot in the given configuration
        for i in range(4):
            pyb.resetJointState(
                self.robot_id, i+1, q[i], physicsClientId = self.col_id
            )


        # compute shortest distances between all object pairs
        distances = []
        for a, b in self.indexed_collision_pairs:
            closest_points = pyb.getClosestPoints(
                a.body_uid,
                b.body_uid,
                distance=max_distance,
                linkIndexA=a.link_uid,
                linkIndexB=b.link_uid,
                physicsClientId=self.col_id,
            )


            # if bodies are above max_distance apart, nothing is returned, so
            # we just saturate at max_distance. Otherwise, take the minimum
            if len(closest_points) == 0:
                distances.append(max_distance)
            else:
                distances.append(np.min([pt[8] for pt in closest_points])) # contact distance, positive for separation, negative for penetration

        return np.array(distances)


    def print_collision_pairs(self, d):
        names = []
        idx = 0
        for a, b in self.indexed_collision_pairs:
                pair = []
                if a.body_uid == 0:
                    first = 'robot end effector'
                    if b.body_uid == 0 and (b.link_uid == 0 or b.link_uid == 1):
                        second = 'robot base'
                    else:
                        second = list(self.bodies.keys())[b.body_uid]
                    if d[idx] > 0:
                        pair = [first, second, "SAFE"]
                    else:
                        pair = [first, second, "IN COLLISION"]
                    idx = idx + 1
                names.append(pair) 

        return names

    def in_collision(self, q, margin=0.0):
        """Returns True if configuration q is in collision, False otherwise.

        Parameters:
          q: Iterable representing the desired configuration.
          margin: Distance at which objects are considered in collision.
             Default is 0.0.
        """
        ds = self.compute_distances(q, max_distance=margin)
        return (ds < margin).any()

dobot_kinematics/dobot_kinematics/collision_utils.py
jkaniuka/magician_ros2
from PyKDL import Chain, Vector, Rotation, Frame, Joint, Segment, JntArray, ChainFkSolverPos_recursive
from math import radians

def calc_FwdKin(theta1, theta2, theta3):

	# Kinematic chain

	chain = Chain()
	base_link__link_1 = Joint(Joint.RotZ) 
	frame1 = Frame(Rotation.RPY(0, 0, 0),
		Vector(0, 0, 0)) 
	segment1 = Segment(base_link__link_1, frame1)
	chain.addSegment(segment1) 


	link_1__link_2 = Joint(Joint.RotY) 
	frame2 = Frame(Rotation.RPY(0, 0, 0),
		Vector(0, 0, 0.135))
	segment2=Segment(link_1__link_2, frame2)
	chain.addSegment(segment2)


	link_2__link_3 = Joint(Joint.RotY) 
	frame3 = Frame(Rotation.RPY(0, 0, 0),
		Vector(0.147, 0, 0))
	segment3=Segment(link_2__link_3,frame3)
	chain.addSegment(segment3)


	# Forward kinematics
	joint_angles=JntArray(3)
	joint_angles[0]= radians(theta1)
	joint_angles[1]= radians(theta2)
	joint_angles[2]= -(radians(theta2) - radians(theta3))


	# Recursive solver of simple kinematics

	fk=ChainFkSolverPos_recursive(chain)
	finalFrame=Frame()
	fk.JntToCart(joint_angles,finalFrame)
	xyz = finalFrame.p 

	return xyz*1000


dobot_kinematics/dobot_kinematics/dobot_forward_kin.py
jkaniuka/magician_ros2
from math import atan2, degrees, sqrt, acos, radians, degrees

def calc_inv_kin(x, y, z, r):
    try:
        angles = [0.0, 0.0, 0.0, 0.0]
        rear_arm = 135
        forearm = 147 

        angles[3] = radians(r)

        angles[0] = atan2(y,x)

        hypotenuse = pow(x,2) + pow(y,2) + pow(z,2)
        beta = atan2(z, sqrt(pow(x,2) + pow(y,2)))
        psi = abs(acos(( hypotenuse + pow(rear_arm,2) - pow(forearm,2) ) / ( 2 * rear_arm * sqrt(hypotenuse) )))
        if beta >= 0:
            angles[1] = radians(90) - (beta + psi)
        elif beta < 0:
            angles[1] = radians(90) - (psi - abs(beta))

        theta_3 = abs(acos(( hypotenuse - pow(rear_arm,2) - pow(forearm,2) ) / ( 2 * rear_arm*  forearm)))
        angles[2] = theta_3 - radians(90) 

        dobot_angles = [ degrees(angles[0]), degrees(angles[1]), degrees(angles[2]) + degrees(angles[1]), degrees(angles[3])] # in Dobot convention
    except:
        return False

    return dobot_angles



dobot_kinematics/dobot_kinematics/dobot_inv_kin.py
jkaniuka/magician_ros2
from dobot_msgs.srv import EvaluatePTPTrajectory
import rclpy
from rclpy.node import Node

# The example concerns the case where the target of the motion was set as a point in Cartesian space.

class PoseValidatorClient(Node):

    def __init__(self):
        super().__init__('dobot_trajectory_validation_cli')
        self.client_validate_goal = self.create_client(EvaluatePTPTrajectory, 'dobot_PTP_validation_service')
        while not self.client_validate_goal.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = EvaluatePTPTrajectory.Request()

    def send_request_check_trajectory(self):
        self.req_validate = EvaluatePTPTrajectory.Request()
        self.req_validate.target = [10.0, 20.0, 30.0, 40.0]
        self.req_validate.motion_type = 4
        self.future_validate = self.client_validate_goal.call_async(self.req_validate)
        rclpy.spin_until_future_complete(self, self.future_validate)
        return self.future_validate.result()


def main(args=None):
    rclpy.init(args=args)
    minimal_client = PoseValidatorClient()
    validation_response = minimal_client.send_request_check_trajectory()
    minimal_client.get_logger().info(
    'Result of calling service: %s' %(validation_response))
    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()



dobot_kinematics/dobot_kinematics/trajectory_validator_client.py
jkaniuka/magician_ros2
from dobot_msgs.srv import EvaluatePTPTrajectory
import rclpy
from rclpy.node import Node
from dobot_kinematics.dobot_inv_kin import calc_inv_kin
from dobot_kinematics.dobot_forward_kin import calc_FwdKin
import math
from rcl_interfaces.msg import SetParametersResult, ParameterDescriptor
import os.path as path
from geometry_msgs.msg import PoseStamped 
import tf_transformations
from dobot_kinematics.collision_detection_server import PyBulletCollisionServer
import numpy as np
import os




class PoseValidatorService(Node):

    def __init__(self):
        super().__init__('dobot_trajectory_validation_server')
        self.srv_PTP = self.create_service(EvaluatePTPTrajectory, 'dobot_PTP_validation_service', self.PTP_trajectory_callback)
        self.subscription_TCP = self.create_subscription(PoseStamped, 'dobot_TCP', self.tcp_position_callback, 10)
        self.collision_server = PyBulletCollisionServer()


        # TCP pose before motion execution - in order to determine equation of linear trajectory
        self.dobot_pose = [] 

        self.trajectory_points = []
        self.max_velocity = 115 #TODO [mm/s]
        self.axis_1_range = {"min": -125, "max": 125}
        self.axis_2_range = {"min": -5, "max": 90}
        self.axis_3_range = {"min": -15, "max": 70}
        self.axis_4_range = {"min": -150, "max": 150}

        self.path_to_collision_model = None
        self.prevent_collision_with_ground = None

        # User-defined axis limits

        # Axis 1
        self.declare_parameter('axis_1_range', rclpy.Parameter.Type.INTEGER_ARRAY, 
        ParameterDescriptor(description = "The range of allowed values for the joint angle of the first axis.", 
        additional_constraints = "The value must be between -125 and 125 degrees."))

        # Axis 2
        self.declare_parameter('axis_2_range', rclpy.Parameter.Type.INTEGER_ARRAY, 
        ParameterDescriptor(description = "The range of allowed values for the joint angle of the second axis.", 
        additional_constraints = "The value must be between -5 and 90 degrees."))

        # Axis 3
        self.declare_parameter('axis_3_range', rclpy.Parameter.Type.INTEGER_ARRAY, 
        ParameterDescriptor(description = "The range of allowed values for the joint angle of the third axis.", 
        additional_constraints = "The value must be between -15 and 70 degrees."))

        # Axis 4 
        self.declare_parameter('axis_4_range', rclpy.Parameter.Type.INTEGER_ARRAY, 
        ParameterDescriptor(description = "The range of allowed values for the joint angle of the fourth axis.", 
        additional_constraints = "The value must be between -150 and 150 degrees."))

        # Additional parameters used for collision detection


        # Tool type
        self.declare_parameter('use_ground_collision_detection', rclpy.Parameter.Type.BOOL, 
        ParameterDescriptor(description = "If ground collision detection is on, then you cannot command the movement to the point where the end tool hits the ground. This option is especially useful during laboratory classes and for beginners :-)"))


        self.add_on_set_parameters_callback(self.parameters_callback)


    def parameters_callback(self, params):

        for param in params:
            if param.name == 'axis_1_range':
                self.axis_1_range["min"] = param.value[0]
                self.axis_1_range["max"] = param.value[1]
                return SetParametersResult(successful=True)
            elif param.name == 'axis_2_range':
                self.axis_2_range["min"] = param.value[0]
                self.axis_2_range["max"] = param.value[1]
                return SetParametersResult(successful=True)
            elif param.name == 'axis_3_range':
                self.axis_3_range["min"] = param.value[0]
                self.axis_3_range["max"] = param.value[1]
                return SetParametersResult(successful=True)
            elif param.name == 'axis_4_range':
                self.axis_4_range["min"] = param.value[0]
                self.axis_4_range["max"] = param.value[1]
                return SetParametersResult(successful=True)
            elif param.name == "use_ground_collision_detection":
                if param.value == True:
                    self.prevent_collision_with_ground = True
                elif param.value == False:
                    self.prevent_collision_with_ground = False
                return SetParametersResult(successful=True)
            else:
                return SetParametersResult(successful=False)



    def tcp_position_callback(self, msg):
            quat = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
            _, _, y = tf_transformations.euler_from_quaternion(quat)
            self.dobot_pose = [float(msg.pose.position.x)*1000, float(msg.pose.position.y)*1000, float(msg.pose.position.z)*1000, float(math.degrees(y))]


    def PTP_trajectory_callback(self, request, response):
        (response.is_valid, response.message) = self.is_target_valid(request.target, request.motion_type)
        return response



    def are_angles_in_range(self, angles):
        if (self.axis_1_range["min"] < angles[0] < self.axis_1_range["max"]) and \
           (self.axis_2_range["min"] < angles[1] < self.axis_2_range["max"]) and \
           (self.axis_3_range["min"] < angles[2] < self.axis_3_range["max"]) and \
           (self.axis_4_range["min"] < angles[3] < self.axis_4_range["max"]):
           return True
        return False


    def is_target_valid(self, target, target_type):

        # Target expressed in joint coordinates
        if target_type == 4:
            in_limit = self.are_angles_in_range(target)
            if in_limit == False:
                return (False, 'Joint limits violated')
            is_trajectory_safe = self.collision_server.validate_trajectory(motion_type = target_type, current_pose = self.dobot_pose, target_point = target, detect_ground = self.prevent_collision_with_ground)
            if is_trajectory_safe == False:
                return (False, 'A collision was detected during trajectory validation. The movement cannot be executed.')
            else:
                return (True, 'Trajectory is safe and feasible.')
            
        elif target_type == 5:
            xyz = target.tolist()
            cartesian_target = calc_FwdKin(xyz[0], xyz[1], xyz[2])
            cartesian_target_point = [float(cartesian_target[0]), float(cartesian_target[1]), float(cartesian_target[2])]
            waypoints = self.collision_server.linear_trajecory_to_discrete_waypoints(self.dobot_pose, cartesian_target_point)
            cartesian_target_list = [float(cartesian_target[0]), float(cartesian_target[1]), float(cartesian_target[2]), xyz[3]]
            for point in waypoints:
                end_tool_rotation = target.tolist()[3]
                point.append(end_tool_rotation)
                angles = calc_inv_kin(*point)
                if angles == False:
                    return (False, 'Inv Kin solving error!')
                in_limit = self.are_angles_in_range(angles)
                if in_limit == False:
                    return (False, 'Joint limits violated')
            is_trajectory_safe = self.collision_server.validate_trajectory(motion_type = target_type, current_pose = self.dobot_pose, target_point = cartesian_target_list, detect_ground = self.prevent_collision_with_ground)
            if is_trajectory_safe == False:
                return (False, 'A collision was detected during trajectory validation. The movement cannot be executed.')
            else:
                return (True, 'Trajectory is safe and feasible.')


        # Target expressed in cartesian coordinates
        elif target_type == 1:
            angles = calc_inv_kin(*target)
            if angles == False:
                return (False, 'Inv Kin solving error!')
            in_limit = self.are_angles_in_range(angles)
            if in_limit == False:
                return (False, 'Joint limits violated')
            is_trajectory_safe = self.collision_server.validate_trajectory(motion_type = target_type, current_pose = self.dobot_pose, target_point = target, detect_ground = self.prevent_collision_with_ground)
            if is_trajectory_safe == False:
                return (False, 'A collision was detected during trajectory validation. The movement cannot be executed.')
            else:
                return (True, 'Trajectory is safe and feasible.')


        elif target_type == 2:
            waypoints = self.collision_server.linear_trajecory_to_discrete_waypoints(self.dobot_pose, target)
            for point in waypoints:
                end_tool_rotation = target.tolist()[3]
                point.append(end_tool_rotation)
                angles = calc_inv_kin(*point)
                if angles == False:
                    return (False, 'Inv Kin solving error!')
                in_limit = self.are_angles_in_range(angles)
                if in_limit == False:
                    return (False, 'Joint limits violated')
            is_trajectory_safe = self.collision_server.validate_trajectory(motion_type = target_type, current_pose = self.dobot_pose, target_point = target, detect_ground = self.prevent_collision_with_ground)
            if is_trajectory_safe == False:
                return (False, 'A collision was detected during trajectory validation. The movement cannot be executed.')
            else:
                return (True, 'Trajectory is safe and feasible.')

        elif target_type == "continuous_path": 
            #TODO
            pass
        else:
            return (False, 'Wrong trajectory type!')


def main(args=None):
    rclpy.init(args=args)

    minimal_service = PoseValidatorService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_kinematics/dobot_kinematics/trajectory_validator_server.py
jkaniuka/magician_ros2
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    config = os.path.join(
        get_package_share_directory('dobot_kinematics'),
        'config',
        'axis_limits.yaml'
        )

    return LaunchDescription([
    Node(
        package='dobot_kinematics',
        executable='trajectory_validator_server',
        output='log',
    ),
    ExecuteProcess(
        cmd=[[
            'sleep 5;','ros2 ', 'param ', 'load ', '/dobot_trajectory_validation_server ', config
        ]],
        shell=True
    )
    ])


dobot_kinematics/launch/dobot_validate_trajectory.launch.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_kinematics'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Trajectory validation with collision detection',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'trajectory_validator_client = dobot_kinematics.trajectory_validator_client:main',
            'trajectory_validator_server = dobot_kinematics.trajectory_validator_server:main',
        ],
    },
)

dobot_kinematics/setup.py
jkaniuka/magician_ros2
import rclpy
from rcl_interfaces.msg import ParameterDescriptor, IntegerRange


def declare_PTP_params(self):
    
    # JOINT 1 
    self.declare_parameter('JT1_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Velocity of the first joint in deg/s.", 
    additional_constraints = "For a 500g load, the speed value must be less than 300 and for 250g load less than 320.",
    integer_range = [IntegerRange(from_value = 1, to_value = 320, step = 0)]))

    self.declare_parameter('JT1_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Acceleration of the first joint in deg/s^2.", 
    additional_constraints = "The acceleration value must be less than 300.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    # JOINT 2
    self.declare_parameter('JT2_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Velocity of the second joint in deg/s.", 
    additional_constraints = "For a 500g load, the speed value must be less than 300 and for 250g load iless than 320.",
    integer_range = [IntegerRange(from_value = 1, to_value = 320, step = 0)]))

    self.declare_parameter('JT2_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Acceleration of the second joint in deg/s^2.", 
    additional_constraints = "The acceleration value must be less than 300.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    # JOINT 3 
    self.declare_parameter('JT3_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Velocity of the third joint in deg/s.", 
    additional_constraints = "For a 500g load, the speed value must be less than 300 and for 250g load less than 320.",
    integer_range = [IntegerRange(from_value = 1, to_value = 320, step = 0)]))

    self.declare_parameter('JT3_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Acceleration of the third joint in deg/s^2.", 
    additional_constraints = "The acceleration value must be less than 300.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    # JOINT 4
    self.declare_parameter('JT4_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Velocity of the fourth joint in deg/s.", 
    additional_constraints = "For a 500g load, the speed value must be less than 300 and for 250g load less than 480.",
    integer_range = [IntegerRange(from_value = 1, to_value = 480, step = 0)]))

    self.declare_parameter('JT4_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Acceleration of the fourth joint in deg/s^2.", 
    additional_constraints = "The acceleration value must be less than 300.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    ##########################################################################################################

    # Velocity
    self.declare_parameter('TCP_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Linear velocity of tool center point (TCP) in mm/s.", 
    additional_constraints = "Must be below 300 mm/s.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    self.declare_parameter('end_tool_rot_vel', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Rotational velocity of end effector in deg/s.", 
    additional_constraints = "Must be below 300 deg/s for 500g workload and below 480 deg/s for 250g workload.",
    integer_range = [IntegerRange(from_value = 1, to_value = 480, step = 0)]))

    # Acceleration
    self.declare_parameter('TCP_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Linear acceleration of tool center point (TCP) in mm/s^2.", 
    additional_constraints = "Must be below 400 mm/s^2.",
    integer_range = [IntegerRange(from_value = 1, to_value = 400, step = 0)]))

    self.declare_parameter('end_tool_rot_acc', rclpy.Parameter.Type.INTEGER, 
    ParameterDescriptor(description="Rotational acceleration of end effector in deg/s^2.", 
    additional_constraints = "Must be below 300 mm/s^2.",
    integer_range = [IntegerRange(from_value = 1, to_value = 300, step = 0)]))

    #########################################################################################################

    self.joint_params_names = ['JT1_vel', 'JT2_vel', 'JT3_vel', 'JT4_vel', 'JT1_acc', 'JT2_acc', 'JT3_acc', 'JT4_acc']
    self.cartesian_params_names = ['TCP_vel', 'end_tool_rot_vel', 'TCP_acc', 'end_tool_rot_acc']

    self.joint_params_dict = {}
    self.cartesian_params_dict = {}



dobot_motion/dobot_motion/PTP_params_class.py
jkaniuka/magician_ros2
import time
import math
from dobot_msgs.action import PointToPoint
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped 
import tf_transformations
from dobot_driver.dobot_handle import bot
from .PTP_params_class import declare_PTP_params
from rcl_interfaces.msg import SetParametersResult
from threading import Thread
from dobot_msgs.srv import EvaluatePTPTrajectory
from dobot_msgs.msg import DobotAlarmCodes

class DobotPTPServer(Node):

    def __init__(self):
        super().__init__('dobot_PTP_server')

        self._action_server = ActionServer(
            self,
            PointToPoint,
            'PTP_action',
            execute_callback=self.execute_callback,
            callback_group=ReentrantCallbackGroup(),
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback)

        self.subscription_joints = self.create_subscription(
            JointState,
            'dobot_joint_states',
            self.joints_positions_callback,
            10)

        self.subscription_TCP = self.create_subscription(
            PoseStamped,
            'dobot_TCP',
            self.tcp_position_callback,
            10)
        
        self.subscription_alarms = self.create_subscription(
            DobotAlarmCodes,
            'dobot_alarms',
            self.active_alarms_callback,
            10)

        # Check if goal/trajectory is feasible
        self.client_validate_goal = self.create_client(srv_type = EvaluatePTPTrajectory, 
                                                        srv_name = 'dobot_PTP_validation_service', 
                                                        callback_group=ReentrantCallbackGroup())
        while not self.client_validate_goal.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Trajectory validation service not available, waiting again...')


        self.motion_type = None 
        self.target = [] 
        self.pose_arr = []
        self.dobot_pose = [] 
        self.mode_ACK = False

        self.motion_types_list = [1,2,4,5]

        declare_PTP_params(self)

        self.set_joint_params_thread = Thread(target = self.set_joint_params_func)
        self._is_running_joint_params = True
        self.allow_dynamic_reconfigure_joints = False

        self.set_cartesian_params_thread = Thread(target = self.set_cartesian_params_func)
        self._is_running_cartesian_params = True
        self.allow_dynamic_reconfigure_cartesian = False

        self.joints_sorted = False
        self.cartesian_sorted = False

        self.ready_to_set_second_part = False

        self.active_alarms = False


        self.add_on_set_parameters_callback(self.parameters_callback)

    def set_initial_params_values(self):
            self.set_joint_params_thread.start()
            self.set_cartesian_params_thread.start()

    def send_joint_parameters(self):
            params = self.sort_params_dict(self.joint_params_dict, "joint")
            bot.set_point_to_point_joint_params(params[0:4], params[4:8])

    def send_cartesian_parameters(self):
            params = self.sort_params_dict(self.cartesian_params_dict, "cartesian")
            bot.set_point_to_point_coordinate_params(params[0], params[1], params[2], params[3])


    def set_joint_params_func(self):
        while(self._is_running_joint_params):
            if len(self.joint_params_dict) == 8:
                self.send_joint_parameters()
                self._is_running_joint_params = False
                self.allow_dynamic_reconfigure_joints = True
                self.ready_to_set_second_part = True
                

    def set_cartesian_params_func(self):
        while(self._is_running_cartesian_params):
            if len(self.cartesian_params_dict) == 4 and self.ready_to_set_second_part:
                self.send_cartesian_parameters()
                self._is_running_cartesian_params = False
                self.allow_dynamic_reconfigure_cartesian = True



    # Sort the parameters in the order necessary to send the command.
    # The user may enter parameters in the YAML file in a different order than the default.
    def sort_params_dict(self, params_dict, params_type):
        if params_type == "joint":
            if not self.joints_sorted:
                new_dict = {}
                for idx in range(len(self.joint_params_dict)):
                    new_dict[self.joint_params_names[idx]] = params_dict[self.joint_params_names[idx]]
                self.joints_sorted = True 
                return list(new_dict.values())
            else:
                return list(self.joint_params_dict.values())

        elif params_type == "cartesian":
            if not self.cartesian_sorted:
                new_dict = {}
                for idx in range(len(self.cartesian_params_dict)):
                    new_dict[self.cartesian_params_names[idx]] = params_dict[self.cartesian_params_names[idx]]
                self.cartesian_sorted = True
                return list(new_dict.values())
            else:
                return list(self.cartesian_params_dict.values())



    def parameters_callback(self, params):
        for param in params:
            if param.name in self.joint_params_names:
                self.joint_params_dict[param.name] = param.value
                if self.allow_dynamic_reconfigure_joints:
                    self.send_joint_parameters()
               
            elif param.name in self.cartesian_params_names:
                self.cartesian_params_dict[param.name] = param.value
                if self.allow_dynamic_reconfigure_cartesian:
                    self.send_cartesian_parameters()

            else:
                return SetParametersResult(successful=False)

        return SetParametersResult(successful=True)

    def send_request_check_trajectory(self, target_point, type):
        self.req_validate = EvaluatePTPTrajectory.Request()
        self.req_validate.target = target_point
        self.req_validate.motion_type = type
        self.future = self.client_validate_goal.call_async(self.req_validate)
        while not self.future.done():
            pass
        return self.future.result()


    def joints_positions_callback(self, msg):
        if self.motion_type in [3, 4, 5, 6]:
            self.dobot_pose = [math.degrees(msg.position[0]), math.degrees(msg.position[1]), math.degrees(msg.position[2]), math.degrees(msg.position[3])]
            self.mode_ACK = True



    def tcp_position_callback(self, msg):
        if self.motion_type in [0, 1, 2, 7, 8, 9]:
            quat = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
            _, _, y = tf_transformations.euler_from_quaternion(quat)
            self.dobot_pose = [float(msg.pose.position.x)*1000, float(msg.pose.position.y)*1000, float(msg.pose.position.z)*1000, float(math.degrees(y))]
            self.mode_ACK = True


    def active_alarms_callback(self, msg):
        if not msg.alarms_list:
            self.active_alarms = False
        else:
            self.active_alarms = True

    
    def destroy(self):
        self._action_server.destroy()
        super().destroy_node()

    @staticmethod
    def is_ratio_valid(ratio):
        if 0.0 < ratio <= 1.0 and int(ratio * 100) != 0:
            return True
        return False


    def goal_callback(self, goal_request):
        """Accept or reject a client request to begin an action."""
        self.target = goal_request.target_pose
        self.motion_type = goal_request.motion_type

        # Check if there are active alarms (if the LED diode lights up red) 
        if self.active_alarms:
            self.get_logger().warn("Goal rejected because of active alarms (LED diode in the robot base lights up red)")
            return GoalResponse.REJECT

        validation_response = self.send_request_check_trajectory(self.target, self.motion_type)
        if validation_response.is_valid == False:
            self.get_logger().warn("Goal rejected: {0}".format(validation_response))
            return GoalResponse.REJECT

        self.get_logger().info("Result of calling validation service: is valid? {0}, description: {1}".format(validation_response.is_valid, validation_response.message))



        if DobotPTPServer.is_ratio_valid(goal_request.velocity_ratio) and DobotPTPServer.is_ratio_valid(goal_request.acceleration_ratio):
            vel_ratio = int(goal_request.velocity_ratio * 100)
            acc_ratio = int(goal_request.acceleration_ratio * 100)
            self.get_logger().info("Vel ratio: {0}".format(vel_ratio))
            self.get_logger().info("Acc ratio: {0}".format(acc_ratio))
            bot.set_point_to_point_common_params(vel_ratio, acc_ratio)
        else:
            self.get_logger().info('Wrong ratio in action goal field')
            return GoalResponse.REJECT

        while not self.mode_ACK:
            pass
        self.get_logger().info('Goal: {0}'.format(self.target))
        self.get_logger().info('Mode: {0}'.format(self.motion_type))
        self.get_logger().info('Received goal request')
        if self.motion_type in self.motion_types_list:
            return GoalResponse.ACCEPT 
        else:
            self.get_logger().info('The motion mode you specified does not exist!')
            return GoalResponse.REJECT

    def cancel_callback(self, goal_handle):
        """Accept or reject a client request to cancel an action."""
        self.get_logger().info('Received cancel request')
        return CancelResponse.ACCEPT

    @staticmethod
    def is_goal_reached(target_pose, current_pose, threshold):
        for i in range(4):
            if abs(target_pose[i]-current_pose[i]) > threshold:
                return False
        return True

    @staticmethod
    def is_pose_stable(pose_arr):
        if len(pose_arr) >= 2:
            if pose_arr[-1] == pose_arr[-2]:
                return True
        return False


    async def execute_callback(self, goal_handle):
        """Execute a goal."""
        self.get_logger().info('Executing goal...')

        bot.set_point_to_point_command(self.motion_type, self.target[0], self.target[1], self.target[2], self.target[3])

        feedback_msg = PointToPoint.Feedback()
        feedback_msg.current_pose = [0.0, 0.0, 0.0, 0.0]

        result = PointToPoint.Result()

        # Start executing the action
        while not (DobotPTPServer.is_goal_reached(self.target, self.dobot_pose, 0.2) and DobotPTPServer.is_pose_stable(self.pose_arr)):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                bot.stop_queue(force=True) 
                bot.clear_queue()
                bot.start_queue()
                self.get_logger().info('Goal canceled')
                result.achieved_pose  = self.dobot_pose
                return result


            # Update sequence
            feedback_msg.current_pose = self.dobot_pose
            self.pose_arr.append(self.dobot_pose)

            self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.current_pose))

            # Publish the feedback
            goal_handle.publish_feedback(feedback_msg)

            # Sleep for demonstration purposes
            time.sleep(0.1)


        goal_handle.succeed()


        result.achieved_pose  = self.dobot_pose

        self.get_logger().info('Returning result: {0}'.format(result.achieved_pose))

        return result


def main(args=None):
    rclpy.init(args=args)

    minimal_action_server = DobotPTPServer()
    minimal_action_server.set_initial_params_values()

    # Use a MultiThreadedExecutor to enable processing goals concurrently
    executor = MultiThreadedExecutor()

    rclpy.spin(minimal_action_server, executor=executor)

    minimal_action_server.destroy()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

dobot_motion/dobot_motion/PTP_server.py
jkaniuka/magician_ros2
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    config = os.path.join(
        get_package_share_directory('dobot_motion'),
        'config',
        'PTP_motion_params.yaml'
        )

    return LaunchDescription([
        Node(
            package='dobot_motion',
            executable='PTP_server',
            output='screen',
        ),
    ExecuteProcess(
        cmd=[[
            'sleep 5;', 'ros2 param load /dobot_PTP_server ', config
        ]],
        shell=True
    )
    ])
dobot_motion/launch/dobot_PTP.launch.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_motion'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*.yaml'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Dobot Magician motion execution using ROS 2 actions.',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'PTP_server = dobot_motion.PTP_server:main',
        ],
    },
)

dobot_motion/setup.py
jkaniuka/magician_ros2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped 
import tf_transformations
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import math
from contextlib import suppress
from dobot_msgs.msg import GripperStatus, DobotAlarmCodes
import os
from std_msgs.msg import Float64

from dobot_driver.dobot_handle import bot

class DobotPublisher(Node):

    def __init__(self):
        super().__init__('dobot_state_publisher')
        self.publisher_joints = self.create_publisher(JointState, 'dobot_joint_states', 10)
        self.publisher_joints_rviz = self.create_publisher(JointState, 'joint_states', 10)
        self.publisher_TCP = self.create_publisher(PoseStamped, 'dobot_TCP', 10)
        self.publisher_alarms = self.create_publisher(DobotAlarmCodes, 'dobot_alarms', 10)
        self.subscription = self.create_subscription(GripperStatus, 'gripper_status_rviz', self.listener_callback, 10)
        timer_period = 0.05  # 50ms = 20Hz
        self.timer = self.create_timer(timer_period, self.timer_callback)
        timer_period_alarms = 0.1 # 100ms = 10 Hz
        self.timer_alarms = self.create_timer(timer_period_alarms, self.timer_callback_alarms)
        self.br = TransformBroadcaster(self)
        self.gripper_width = 0.0
        self.RAIL_IN_USE = False

        if os.environ.get('MAGICIAN_RAIL_IN_USE') == "true":
            self.RAIL_IN_USE = True
            self.publisher_rail = self.create_publisher(Float64, 'dobot_rail_pose', 10)

    def listener_callback(self, msg):
        if msg.status == "opened":
            self.gripper_width = 0.0135
        elif msg.status == "closed":
            self.gripper_width = 0.0

    def ignore_msg(self):
        bot.serial.reset_input_buffer()

    @staticmethod
    def check_received_msg_type(msg):
        for digit in msg:
            if isinstance(digit, float):
                return False
        return True

    def timer_callback_alarms(self):
        with suppress(ValueError, TypeError):
            alarms_array = bot.get_alarms_state()
            # self.get_logger().info("Raw alarms:  {0}".format(alarms_array))
            if (self.RAIL_IN_USE == True) and (isinstance(alarms_array, float)):
                self.timer_alarms.reset()
            if alarms_array == None:
                self.ignore_msg()
            if alarms_array != None:
                if DobotPublisher.check_received_msg_type(alarms_array):
                    alarms_msg = DobotAlarmCodes()
                    alarms_msg.header.stamp = self.get_clock().now().to_msg()
                    for i, x in enumerate(alarms_array):
                        for j in range(8):
                            if x & (1<<j) > 0:
                                alarm_value = 8*i + j
                                alarms_msg.alarms_list.append(int(f'{alarm_value:x}'))
                    self.publisher_alarms.publish(alarms_msg)
                else:
                    self.ignore_msg()


    def timer_callback(self):

        with suppress(ValueError, TypeError):
            [x, y, z, r, theta1, theta2, theta3, theta4] = bot.get_pose()

            joint_state = JointState()
            now = self.get_clock().now()
            joint_state.header.stamp = now.to_msg()
            joint_state.name = ['magician_joint_1', 'magician_joint_2', 'magician_joint_3', 'magician_joint_4', 'magician_joint_prismatic_l']
            joint_state.position = [math.radians(theta1), math.radians(theta2), math.radians(theta3 - theta2), math.radians(theta4), self.gripper_width]
            # self.get_logger().info("JT1:{0} JT2:{1} JT3:{2} JT4:{3}".format(theta1, theta2, theta3, theta4))
            self.publisher_joints_rviz.publish(joint_state)
            joint_state.position = [math.radians(theta1), math.radians(theta2), math.radians(theta3), math.radians(theta4), self.gripper_width]
            self.publisher_joints.publish(joint_state)

            ####################################################
            dobot_TCP = PoseStamped()
            dobot_TCP.header.stamp = self.get_clock().now().to_msg()
            dobot_TCP.pose.position.x = x/1000
            dobot_TCP.pose.position.y = y/1000
            dobot_TCP.pose.position.z = z/1000

            q = tf_transformations.quaternion_from_euler(0, 0, r * math.pi/180)
            dobot_TCP.pose.orientation.x = q[0]
            dobot_TCP.pose.orientation.y = q[1]
            dobot_TCP.pose.orientation.z = q[2]
            dobot_TCP.pose.orientation.w = q[3]
            self.publisher_TCP.publish(dobot_TCP)
            ####################################################

            t = TransformStamped()
            t.header.stamp = self.get_clock().now().to_msg()
            t.header.frame_id = 'magician_base_link'
            t.child_frame_id = 'TCP'
            t.transform.translation.x = x/1000
            t.transform.translation.y = y/1000
            t.transform.translation.z = z/1000

            # self.get_logger().info("x:{0} y:{1} z:{2} r:{3}".format(x/1000, y/1000, z/1000, r))

            q = tf_transformations.quaternion_from_euler(0, 0, r * math.pi/180)
            t.transform.rotation.x = q[0]
            t.transform.rotation.y = q[1]
            t.transform.rotation.z = q[2]
            t.transform.rotation.w = q[3]

            self.br.sendTransform(t)

            if self.RAIL_IN_USE == True:
                dobot_rail_pose = Float64()
                dobot_rail_pose.data = float(bot.get_sliding_rail_pose())
                self.publisher_rail.publish(dobot_rail_pose)

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = DobotPublisher()

    rclpy.spin(minimal_publisher)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_state_updater/dobot_state_updater/dobot_state_publ.py
jkaniuka/magician_ros2
from setuptools import setup

package_name = 'dobot_state_updater'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Receiving information about robot position and alarms',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'state_publisher = dobot_state_updater.dobot_state_publ:main',
        ],
    },
)

dobot_state_updater/setup.py
jkaniuka/magician_ros2
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from ament_index_python.packages import get_package_share_directory

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

import time

class FOVPublisher(Node):

    def __init__(self):
        super().__init__('camera_frustum_visualization')
        self.marker_pub = self.create_publisher(Marker, 'realsense_FOV', 10)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        timer_period = 0.05 # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)


    def timer_callback(self):

        try:
            trans = self.tf_buffer.lookup_transform(
                'magician_root_link',
                'FOV_marker_origin',
                rclpy.time.Time())
        except TransformException as ex:
            self.get_logger().info(
                "Waiting for TF data")
            time.sleep(1)
            return



        #####################################################################################################

        marker = Marker()
        marker.header.frame_id = "magician_root_link"
        marker.id = 0

        marker.type = Marker.MESH_RESOURCE
        package_share_directory = get_package_share_directory('dobot_visualization_tools')
        my_path = "file://" + str(package_share_directory) + "/dae_models/realsense_FOV.dae"
        marker.mesh_resource = my_path
        marker.action = Marker.ADD
        marker.scale.x = 1.0
        marker.scale.y = 1.0
        marker.scale.z = 1.0

        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.6
        marker.color.b = 0.0


        marker.pose.orientation.w = trans.transform.rotation.w
        marker.pose.orientation.x = trans.transform.rotation.x
        marker.pose.orientation.y = trans.transform.rotation.y
        marker.pose.orientation.z = trans.transform.rotation.z

        marker.pose.position.x = trans.transform.translation.x
        marker.pose.position.y = trans.transform.translation.y
        marker.pose.position.z = trans.transform.translation.z

        self.marker_pub.publish(marker)
    

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = FOVPublisher()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_visualization_tools/dobot_visualization_tools/camera_range.py
jkaniuka/magician_ros2
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from ament_index_python.packages import get_package_share_directory

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

import time

class DobotRange(Node):

    def __init__(self):
        super().__init__('dobot_workspace')
        self.marker_pub = self.create_publisher(Marker, 'reachability_range', 10)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):

        from_frame_rel = 'magician_base_link'
        to_frame_rel = 'magician_root_link'

        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform(
                to_frame_rel,
                from_frame_rel,
                now)
        except TransformException as ex:
            self.get_logger().info(
                "Waiting for TF data")
            time.sleep(1)
            return

        marker = Marker()
        marker.header.frame_id = "magician_root_link"
        marker.id = 0

        marker.type = Marker.MESH_RESOURCE
        package_share_directory = get_package_share_directory('dobot_visualization_tools')
        my_path = "file://" + str(package_share_directory) + "/dae_models/dobot_range.dae"
        marker.mesh_resource = my_path
        marker.action = Marker.ADD
        marker.scale.x = 1.0
        marker.scale.y = 1.0
        marker.scale.z = 1.0

        marker.color.a = 0.3
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0


        marker.pose.orientation.w = trans.transform.rotation.w
        marker.pose.orientation.x = trans.transform.rotation.x
        marker.pose.orientation.y = trans.transform.rotation.y
        marker.pose.orientation.z = trans.transform.rotation.z

        marker.pose.position.x = trans.transform.translation.x
        marker.pose.position.y = trans.transform.translation.y
        marker.pose.position.z = trans.transform.translation.z

        self.marker_pub.publish(marker)
    

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = DobotRange()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_visualization_tools/dobot_visualization_tools/dobot_range.py
jkaniuka/magician_ros2
import sys
from interactive_markers import InteractiveMarkerServer, MenuHandler
import rclpy
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl, Marker, InteractiveMarkerFeedback
from rclpy.node import Node
import tf_transformations
from math import degrees, pi, sqrt, cos, sin, atan
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import time
from geometry_msgs.msg import Point
from std_msgs.msg import ColorRGBA
import subprocess
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

class TCPInteractiveMarker(Node):

    def __init__(self):
        super().__init__('TCP_int_marker')

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.timer_callback)
        self.br = StaticTransformBroadcaster(self)

        self.declare_parameter('TCP_x_offset', 0.0)
        self.declare_parameter('TCP_z_offset', 0.0)
        self.TCP_x_offset = self.get_parameter('TCP_x_offset').value
        self.TCP_z_offset = self.get_parameter('TCP_z_offset').value

    def timer_callback(self):

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'magician_link_4'
        t.child_frame_id = 'tool'
        t.transform.translation.x = self.TCP_x_offset #0.059
        t.transform.translation.y = 0.0
        t.transform.translation.z = self.TCP_z_offset #-0.12
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0
        self.br.sendTransform(t)

        try:
            trans = self.tf_buffer.lookup_transform(
                'magician_base_link',
                'tool',
                rclpy.time.Time())
        except TransformException as ex:
            self.get_logger().info(
                "Waiting for TF data")
            time.sleep(1)
            return
        # create an interactive marker server on the namespace TCP_marker
        server = InteractiveMarkerServer(self, 'TCP_int_marker')

        menu_handler = MenuHandler()
        menu_handler.insert('Linear motion', callback=self.processFeedback)
        menu_handler.insert('Joint interpolated motion', callback=self.processFeedback)

        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = 'magician_base_link'
        int_marker.pose.position.x = trans.transform.translation.x
        int_marker.pose.position.y = trans.transform.translation.y
        int_marker.pose.position.z = trans.transform.translation.z

        int_marker.pose.orientation.x = trans.transform.rotation.x
        int_marker.pose.orientation.y = trans.transform.rotation.y
        int_marker.pose.orientation.z = trans.transform.rotation.z
        int_marker.pose.orientation.w = trans.transform.rotation.w
        int_marker.name = 'dobot_TCP_int_marker'

        marker_scale = 0.01
        box_marker = Marker()
        box_marker.type = Marker.SPHERE
        box_marker.scale.x = marker_scale
        box_marker.scale.y = marker_scale
        box_marker.scale.z = marker_scale
        box_marker.color.r = 1.0
        box_marker.color.g = 1.0
        box_marker.color.b = 0.0
        box_marker.color.a = 1.0

        # create a non-interactive control which contains the box
        box_control = InteractiveMarkerControl()
        box_control.always_visible = True
        box_control.markers.append(box_marker)
        # add the control to the interactive marker
        int_marker.controls.append(box_control)



        control = InteractiveMarkerControl()
        control.orientation.w = 1.0
        control.orientation.x = 1.0
        control.orientation.y = 0.0
        control.orientation.z = 0.0
        self.normalizeQuaternion(control.orientation)
        control.name = 'move_x'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        # control.orientation_mode = InteractiveMarkerControl.FIXED


        marker = self.create_control_visualization([0.3, 1.0, 0.0, 0.0], [0.02, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0])
        control.markers.append(marker)
        marker = self.create_control_visualization([0.3, 1.0, 0.0, 0.0], [-0.02, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0])
        control.markers.append(marker)
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1.0
        control.orientation.x = 0.0
        control.orientation.y = 1.0
        control.orientation.z = 0.0
        self.normalizeQuaternion(control.orientation)
        control.name = 'rotate_z'
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        disk = self.create_disk()
        control.markers.append(disk)
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1.0
        control.orientation.x = 0.0
        control.orientation.y = 1.0
        control.orientation.z = 0.0
        self.normalizeQuaternion(control.orientation)
        control.name = 'move_z'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        # control.orientation_mode = InteractiveMarkerControl.FIXED
        marker = self.create_control_visualization([0.3, 0.0, 0.0, 1.0], [0.0, 0.0, 0.02], [sqrt(2), 0.0, -sqrt(2), 0.0])
        control.markers.append(marker)
        marker = self.create_control_visualization([0.3, 0.0, 0.0, 1.0], [0.0, 0.0, -0.02], [sqrt(2), 0.0, sqrt(2), 0.0])
        control.markers.append(marker)
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1.0
        control.orientation.x = 0.0
        control.orientation.y = 0.0
        control.orientation.z = 1.0
        self.normalizeQuaternion(control.orientation)
        control.name = 'move_y'
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        # control.orientation_mode = InteractiveMarkerControl.FIXED

        marker = self.create_control_visualization([0.3, 0.0, 1.0, 0.0], [0.0, 0.02, 0.0], [sqrt(2), 0.0, 0.0, sqrt(2)])
        control.markers.append(marker)
        marker = self.create_control_visualization([0.3, 0.0, 1.0, 0.0], [0.0, -0.02, 0.0], [sqrt(2), 0.0, 0.0, -sqrt(2)])
        control.markers.append(marker)
        int_marker.controls.append(control)


        control = InteractiveMarkerControl()
        control.interaction_mode = InteractiveMarkerControl.MENU
        # control.description = 'MOVE TO POINT'
        marker_scale = 0.05
        marker = Marker()
        marker.type = Marker.SPHERE
        marker.scale.x = marker_scale
        marker.scale.y = marker_scale
        marker.scale.z = marker_scale
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.color.a = 0.0
        control.markers.append(marker)
        int_marker.controls.append(control)

        server.insert(int_marker, feedback_callback=self.processFeedback)
        menu_handler.apply(server, int_marker.name)

        # 'commit' changes and send to all clients
        server.applyChanges()
        self.timer.cancel()
        self.timer.destroy()


    def create_control_visualization(self, color, offset, rotation):
        marker = Marker()
        marker.type = Marker.ARROW
        marker.scale.x = 0.08 #0.1
        marker.scale.y = 0.01
        marker.scale.z = 0.01

        marker.color.a = color[0]
        marker.color.r = color[1]
        marker.color.g = color[2]
        marker.color.b = color[3]

        marker.pose.position.x = offset[0]
        marker.pose.position.y = offset[1]
        marker.pose.position.z = offset[2]

        marker.pose.orientation.w = rotation[0]
        marker.pose.orientation.x = rotation[1]
        marker.pose.orientation.y = rotation[2]
        marker.pose.orientation.z = rotation[3]

        return marker

    def processFeedback(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
            self.get_logger().info(
                f'Menu item {feedback.menu_entry_id} has been clicked'
            )

            if feedback.menu_entry_id == 1:
                motion_type = 2
            elif feedback.menu_entry_id == 2:
                motion_type = 1

            ori = feedback.pose.orientation
            quat = (ori.x, ori.y, ori.z, ori.w)
            _, _, yaw = tf_transformations.euler_from_quaternion(quat)
            x = feedback.pose.position.x 
            y = feedback.pose.position.y 
            z = feedback.pose.position.z 
            r = yaw
            #print(degrees(yaw))
            # print(feedback.menu_entry_id)

            alfa = atan(abs(x/y))
            if x >= 0 and y >= 0:
                goal_x = x - sin(alfa)*self.TCP_x_offset
                goal_y = y - cos(alfa)*self.TCP_x_offset
            elif x >= 0 and y < 0:
                goal_x = x - sin(alfa)*self.TCP_x_offset
                goal_y = y + cos(alfa)*self.TCP_x_offset
            elif x < 0 and y >= 0:
                goal_x = x + sin(alfa)*self.TCP_x_offset
                goal_y = y - cos(alfa)*self.TCP_x_offset
            elif x < 0 and y < 0:
                goal_x = x + sin(alfa)*self.TCP_x_offset
                goal_y = y + cos(alfa)*self.TCP_x_offset

            goal_z = z + self.TCP_z_offset*(-1)

            goal_x = goal_x * 1000
            goal_y = goal_y * 1000
            goal_z = goal_z * 1000
            goal_r = degrees(r)
            #print(goal_x, goal_y, goal_z, goal_r)

            
            command = 'ros2 action send_goal /PTP_action  dobot_msgs/action/PointToPoint "{motion_type: ' + str(motion_type) + ', target_pose: [' + str(goal_x) + ',' + str(goal_y) + ',' + str(goal_z) + ',' + str(goal_r) + '], velocity_ratio: 0.5, acceleration_ratio: 0.3}"'
            subprocess.Popen(
                        command, universal_newlines=True, shell=True,
                        stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()

    def normalizeQuaternion(self, quaternion_msg):
        norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
        s = norm**(-0.5)
        quaternion_msg.x *= s
        quaternion_msg.y *= s
        quaternion_msg.z *= s
        quaternion_msg.w *= s



    def create_disk(self):
        marker = Marker()

        marker.type = Marker.TRIANGLE_LIST
        scale = 0.2
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale

        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0

        marker.pose.orientation.w = sqrt(2)
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = sqrt(2)
        marker.pose.orientation.z = 0.0

        steps = 36
        width = 0.3

        marker.points = [Point()] * 6 * steps

        circle1 = []
        circle2 = []

        for i in range(steps):
            a=i/steps*pi*2
            v1 = Point()
            v2 = Point()
            v1.y = 0.5 * cos(a)
            v1.z = 0.5 * sin(a)

            v2.y = (1 + width) * v1.y
            v2.z = (1 + width) * v1.z

            circle1.append(v1)
            circle2.append(v2)

        color = ColorRGBA()
        color.r = color.g = color.b = color.a = 1.0
        marker.colors = [ColorRGBA()] * 2 * steps 
        base_color = ColorRGBA()
        marker.color.r = 0.0
        marker.color.g = 0.0
        marker.color.b = 1.0
        marker.color.a = 0.5
        base_color = marker.color
    

        for i in range(0,steps-1,2):
            i1 = i
            i2 = (i +1) % steps
            i3 = (i+2) % steps
            p = i * 6
            c = i * 2

            marker.points[p + 0] = circle1[i1]
            marker.points[p + 1] = circle2[i2]
            marker.points[p + 2] = circle1[i2]

            marker.points[p + 3] = circle1[i2]
            marker.points[p + 4] = circle2[i2]
            marker.points[p + 5] = circle1[i3]

            color.r = base_color.r * 0.6
            color.g = base_color.g * 0.6
            color.b = base_color.b * 0.6

            marker.colors[c] = color
            marker.colors[c + 1] = color

            p  = p + 6
            c = c + 2

            marker.points[p + 0] = circle2[i1]
            marker.points[p + 1] = circle2[i2]
            marker.points[p + 2] = circle1[i1]

            marker.points[p + 3] = circle2[i2]
            marker.points[p + 4] = circle2[i3]
            marker.points[p + 5] = circle1[i3]

            marker.colors[c] = base_color
            marker.colors[c + 1] = base_color
            


        return marker

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = TCPInteractiveMarker()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

dobot_visualization_tools/dobot_visualization_tools/int_marker.py
jkaniuka/magician_ros2
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

import time

# Resource: https://github.com/DavidB-CMU/rviz_tools_py/blob/master/src/rviz_tools_py/rviz_tools.py

class TrajectoryPublisher(Node):

    def __init__(self):
        super().__init__('trajectory_visualization')
        self.marker_pub = self.create_publisher(Marker, 'TCP_trajectory', 10)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        timer_period = 0.05 # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.intermediate_points = []
        self.trans_current = [0,0,0]
        self.trans_previous = [0,0,0]
        self.stability_counter = 0

    def erase_if_not_moving(self):
        if self.trans_current == self.trans_previous:
            self.stability_counter = self.stability_counter + 1
        else:
            self.stability_counter = 0

        if self.stability_counter == 40: # 40 x 0.05 sec = 2 sec
            self.stability_counter = 0
            return True
        return False



    def timer_callback(self):

        try:
            trans = self.tf_buffer.lookup_transform(
                'magician_root_link',
                'TCP',
                rclpy.time.Time())
        except TransformException as ex:
            self.get_logger().info(
                "Waiting for TF data")
            time.sleep(1)
            return

        self.trans_current = [trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z]


        ######################################################

        marker = Marker()
        marker.header.frame_id = "magician_root_link"
        marker.id = 0

        marker.type = Marker.SPHERE_LIST
        marker.action = Marker.ADD
        marker.scale.x = 0.01
        marker.scale.y = 0.01
        marker.scale.z = 0.01

        point = Point()
        point.x = trans.transform.translation.x
        point.y = trans.transform.translation.y
        point.z = trans.transform.translation.z

        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
      
        self.intermediate_points.append(point)
        marker.points = self.intermediate_points

        is_pose_stable = self.erase_if_not_moving()

        if is_pose_stable == True:
            marker.action = Marker.DELETE
            self.intermediate_points = []

        self.trans_previous = [trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z]
        self.marker_pub.publish(marker)
    

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = TrajectoryPublisher()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
dobot_visualization_tools/dobot_visualization_tools/trajectory_markers.py
jkaniuka/magician_ros2
from setuptools import setup
import os
from glob import glob

package_name = 'dobot_visualization_tools'
meshes_folder = 'dae_models'

setup(
    name=package_name,
    version='1.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, meshes_folder), glob('dae_models/*'))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Jan Kaniuka',
    maintainer_email='kan.jan@wp.pl',
    description='Useful tools for visualization',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'show_camera_FOV = dobot_visualization_tools.camera_range:main',
            'show_dobot_range = dobot_visualization_tools.dobot_range:main',
            'show_trajectory = dobot_visualization_tools.trajectory_markers:main',
            'int_marker = dobot_visualization_tools.int_marker:main',
        ],
    },
)

dobot_visualization_tools/setup.py
jkaniuka/magician_ros2