Restyling w,x,a,d,s to 8,2,5,4,6 and adding gripper open, close, home and arm-forward poses

[aziyatali]

#!/usr/bin/env python

Copyright (c) 2011, Willow Garage, Inc.

All rights reserved.

Redistribution and use in source and binary forms, with or without

modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright

notice, this list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright

notice, this list of conditions and the following disclaimer in the

documentation and/or other materials provided with the distribution.

* Neither the name of the Willow Garage, Inc. nor the names of its

contributors may be used to endorse or promote products derived from

this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

POSSIBILITY OF SUCH DAMAGE.

from logging.config import valid_ident from ntpath import join import rospy from geometry_msgs.msg import Twist import sys, select, os import copy import moveit_commander import moveit_msgs.msg

if os.name == 'nt': import msvcrt, time else: import tty, termios

from future import print_function from six.moves import input

try: from math import pi, tau, dist, fabs, cos except: # For Python 2 compatibility from math import pi, fabs, cos, sqrt

tau = 2.0 * pi

def dist(p, q):
    return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q)))

from std_msgs.msg import String from moveit_commander.conversions import pose_to_list

BURGER_MAX_LIN_VEL = 0.22 BURGER_MAX_ANG_VEL = 2.84

WAFFLE_MAX_LIN_VEL = 0.26 WAFFLE_MAX_ANG_VEL = 1.82

LIN_VEL_STEP_SIZE = 0.01 ANG_VEL_STEP_SIZE = 0.1

msg = """ Control Your TurtleBot3!

Moving around: 8 4 5 6 2

1 -> home pose 0 -> extend arm

f -> gripper open g -> gripper close w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26) a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit """

e = """ Communications Failed """

def getKey(): if os.name == 'nt': timeout = 0.1 startTime = time.time() while(1): if msvcrt.kbhit(): if sys.version_info[0] >= 3: return msvcrt.getch().decode() else: return msvcrt.getch() elif time.time() - startTime > timeout: return ''

tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
    key = sys.stdin.read(1)
else:
    key = ''

termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key

def vels(target_linear_vel, target_angular_vel): return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def jnts(j1, j2,j3,j4): return "currently:\tjoint1 %s\t joint2 %s\t joint3 %s\t joint3 %s " % (j1,j2,j3,j4) def makeSimpleProfile(output, input, slop): if input > output: output = min( input, output + slop ) elif input < output: output = max( input, output - slop ) else: output = input

return output

def constrain(input, low, high): if input < low: input = low elif input > high: input = high else: input = input

return input

def checkLinearLimitVelocity(vel): if turtlebot3_model == "burger": vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL) elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi": vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL) else: vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)

return vel

def checkAngularLimitVelocity(vel): if turtlebot3_model == "burger": vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL) elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi": vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL) else: vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)

return vel

if name=="main": if os.name != 'nt': settings = termios.tcgetattr(sys.stdin)

moveit_commander.roscpp_initialize(sys.argv) # initialize moveit commander
robot = moveit_commander.RobotCommander() # Instantiate a RobotCommander object. 
#Provides information such as the robot’s kinematic model and the robot’s current joint states
gripper = moveit_commander.MoveGroupCommander('gripper') #name of movegroup 2 is “gripper
arm = moveit_commander.MoveGroupCommander('arm') # name of movegroup 1 is “arm”
arm.set_planning_time(2) # I want to use that too ahahha

rospy.init_node('turtlebot3_teleop')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
display_trajectory_publisher = rospy.Publisher(
"/move_group/display_planned_path",
moveit_msgs.msg.DisplayTrajectory,
queue_size=20,

)

turtlebot3_model = rospy.get_param("model", "burger")

status = 0
target_linear_vel   = 0.0
target_angular_vel  = 0.0
control_linear_vel  = 0.0
control_angular_vel = 0.0
joint_values = arm.get_current_joint_values() #How to get joint states
joint_gripper = gripper.get_current_joint_values() # gripper value
joint_gripper[0] = 0.010
joint_values[0] = 0.0
joint_values[1] = 0.0
joint_values[2] = 0.0
joint_values[3] = 0.0

try:
    print(msg)
    while not rospy.is_shutdown():
        key = getKey()
        if key == '8' :
            target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
            status = status + 1
            print(vels(target_linear_vel,target_angular_vel))
        elif key == '2' :
            target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
            status = status + 1
            print(vels(target_linear_vel,target_angular_vel))
        elif key == '4' :
            target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
            status = status + 1
            print(vels(target_linear_vel,target_angular_vel))
        elif key == '6' :
            target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
            status = status + 1
            print(vels(target_linear_vel,target_angular_vel))
        elif key == '5' :
            target_linear_vel   = 0.0
            control_linear_vel  = 0.0
            target_angular_vel  = 0.0
            control_angular_vel = 0.0
            print(vels(target_linear_vel, target_angular_vel))
        elif key == 'g':
            joint_gripper = gripper.get_current_joint_values()
            joint_gripper[0] = 0.010
            gripper.go(joints=joint_gripper, wait=True)
            rospy.sleep(5)
            arm.stop()
            print("Gripper opened")
            #Please call this if you are getting jerky movements -tip from TA
            gripper.clear_pose_targets()
            #gripper open
        elif key == 'f':
            joint_gripper = gripper.get_current_joint_values()
            joint_gripper[0] = -0.010
            gripper.go(joints=joint_gripper, wait=True)
            rospy.sleep(5)
            arm.stop()
            print("Gripper closed")
            #Please call this if you are getting jerky movements -tip from TA
            gripper.clear_pose_targets()
            #gripper close
        elif key == '0':
            joint_values = arm.get_current_joint_values()
            joint_values[1] = 0.125
            joint_values[2] = 0.257
            joint_values[3] = 0.357
            arm.go(joints=joint_values, wait=True)
            rospy.sleep(5)
            arm.stop()
            print(jnts(joint_values[0], joint_values[1], joint_values[2], joint_values[3]))
            #Please call this if you are getting jerky movements -tip from TA
            #arm.clear_pose_targets()
            #extend arm forward
        elif key == '1':
            joint_values = arm.get_current_joint_values()
            joint_values[0] = 0.0
            joint_values[1] = -1.0
            joint_values[2] = 0.3
            joint_values[3] = 0.7
            arm.go(joints=joint_values, wait=True)
            rospy.sleep(5)
            arm.stop()
            print(jnts(joint_values[0], joint_values[1], joint_values[2], joint_values[3]))
            #Please call this if you are getting jerky movements -tip from TA
            #arm.clear_pose_targets()
            #home pose
        else:
            if (key == 'q'):
                break

        if status == 20 :
            print(msg)
            status = 0

        twist = Twist()

        control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
        twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0

        control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
        twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
        
        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        display_trajectory.trajectory_start = robot.get_current_state()
        #display_trajectory.trajectory.append(plan) --> used in case pose is used
        # Publish
        display_trajectory_publisher.publish(display_trajectory)
        pub.publish(twist)

except:
    print(e)

finally:
    twist = Twist()
    twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
    twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
    pub.publish(twist)

if os.name != 'nt':
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

[aziyatali]

Adding gripper open, close, home pose, arm forward, and restyles w,x,a,d,s to 8,2,4,6,5

[aziyatali]

Up for review

[ROBOTIS-Will]

Hi @aziyatali Thank you for your contribution. Could you please commit your changes and submit the PR for our review?

[aziyatali]

Hi @ROBOTIS-Will,

sorry for the confusion. Currently, we are working with ROS project in school and would like to propose some additional features over existing teleop_key. I committed the changes and it is up for PR review.

[ROBOTIS-Will]

@aziyatali Sure, not a problem. Please note that the teleop_key is widely used for original TurtleBot3 package users, so any changes must be compatible with the original hardware configuration. Thank you!

[aziyatali]

@ROBOTIS-Will Yes, I have launched turtlebot3_navigation before launching the teleop_key to support the moveit_commander package and it worked well. Anyways, looking forward to see this community growing!