Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ur_robot_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ install(TARGETS ur_robot_driver_plugin urcl_log_handler ur_robot_driver_node rob
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

catkin_install_python(PROGRAMS scripts/tool_communication
catkin_install_python(PROGRAMS scripts/tool_communication scripts/test_move
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

install(DIRECTORY config launch resources
Expand Down
19 changes: 11 additions & 8 deletions ur_robot_driver/scripts/test_move
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#!/usr/bin/env python3
#!/usr/bin/env python

# -- BEGIN LICENSE BLOCK ----------------------------------------------
# Copyright 2021 FZI Forschungszentrum Informatik
Expand All @@ -25,7 +25,6 @@
#
#
# ---------------------------------------------------------------------

import sys

import rospy
Expand All @@ -41,6 +40,10 @@ from cartesian_control_msgs.msg import (
CartesianTrajectoryPoint,
)

# Compatibility for python2 and python3
if sys.version_info[0] < 3:
input = raw_input

# If your robot description is created with a tf_prefix, those would have to be adapted
JOINT_NAMES = [
"shoulder_pan_joint",
Expand Down Expand Up @@ -199,7 +202,7 @@ class TrajectoryClient:
)
valid = input_str in ["y", "n"]
if not valid:
print("Please confirm by entering 'y' or abort by entering 'n'")
rospy.loginfo("Please confirm by entering 'y' or abort by entering 'n'")
else:
confirmed = input_str == "y"
if not confirmed:
Expand All @@ -208,11 +211,11 @@ class TrajectoryClient:

def choose_controller(self):
"""Ask the user to select the desired controller from the available list."""
print("Available trajectory controllers:")
rospy.loginfo("Available trajectory controllers:")
for (index, name) in enumerate(JOINT_TRAJECTORY_CONTROLLERS):
print("{} (joint-based): {}".format(index, name))
rospy.loginfo("{} (joint-based): {}".format(index, name))
for (index, name) in enumerate(CARTESIAN_TRAJECTORY_CONTROLLERS):
print("{} (Cartesian): {}".format(index + len(JOINT_TRAJECTORY_CONTROLLERS), name))
rospy.loginfo("{} (Cartesian): {}".format(index + len(JOINT_TRAJECTORY_CONTROLLERS), name))
choice = -1
while choice < 0:
input_str = input(
Expand All @@ -224,13 +227,13 @@ class TrajectoryClient:
if choice < 0 or choice >= len(JOINT_TRAJECTORY_CONTROLLERS) + len(
CARTESIAN_TRAJECTORY_CONTROLLERS
):
print(
rospy.loginfo(
"{} not inside the list of options. "
"Please enter a valid index from the list above.".format(choice)
)
choice = -1
except ValueError:
print("Input is not a valid number. Please try again.")
rospy.loginfo("Input is not a valid number. Please try again.")
if choice < len(JOINT_TRAJECTORY_CONTROLLERS):
self.joint_trajectory_controller = JOINT_TRAJECTORY_CONTROLLERS[choice]
return "joint_based"
Expand Down