def main():
# Process command line arguments
opt = docopt(__doc__)
file_name = opt['<image>']
repeat = opt['--repeat']
# Read the image
image = imread(file_name)
image = image.astype('float32')
# RGB to gray-scale
if len(image.shape) > 2 and image.shape[2] == 3:
image_gray = np.dot(image[..., :3], [0.299, 0.587, 0.144])
else:
image_gray = image
# Resample to 0-255
image_gray = image_gray / np.max(image_gray) * 255
image3 = np.atleast_3d(image_gray)
image3 = image3.astype('uint8')
image_msg = np_to_imgmsg(image3)
print 'Image shape: %s dtype: %s, message enc: %s' % (str(
image.shape), str(image.dtype), image_msg.encoding)
# Initialize ros
rospy.init_node("pubimage", anonymous=True)
# Wait for service to be ready
rospy.loginfo("Waiting for %s service to be ready ...", dotmatrix_service)
rospy.wait_for_service(dotmatrix_service)
# Register and call the service
rospy.loginfo("Service %s is ready, issuing request ...",
dotmatrix_service)
srv = rospy.ServiceProxy(dotmatrix_service, DetectDotmatrix)
try:
x, y = call_service(srv, image_msg)
fig = plt.figure()
ax = fig.add_subplot(111)
points, = ax.plot(x, y, 'xr')
ax.imshow(image_gray)
plt.draw()
if repeat:
t = threading.Thread(target=update_plot,
args=(
srv,
image_msg,
points,
))
t.daemon = True
t.start()
plt.show()
except rospy.ServiceException as exc:
rospy.logerr("Service did not process request: " + str(exc))
sys.exit(1)
def main():
# Process cammand line arguments
argv = rospy.myargv(argv=sys.argv)
opt = docopt(__doc__, argv=argv[1:])
opt_debug = opt['--debug']
opt_trajectory_file = opt['<trajectory_file>']
opt_force = opt['--force']
opt_no_confirm = opt['--no-confirm']
opt_xtion = opt['--xtion']
opt_output = opt['--output']
if opt_debug:
print opt
# Initialise node
rospy.init_node(NODE_NAME, anonymous=NODE_ANONYMOUS)
# Initialise robot
robot = ClopemaRobotCommander(ROBOT_GROUP)
# Initialise tf buffer
tfb = get_tf2_buffer()
# Load trajectories
trajs = read_msgs(opt_trajectory_file, RobotTrajectory)
# Infer what xtion we are calibrating
if opt_xtion is None:
xtion = get_xtion(infer_xtion(trajs))
else:
xtion = get_xtion(int(opt_xtion))
# Check output directory
if opt_output is not None:
output_path = opt_output
if not prepare_output_directory(output_path, opt_force):
rospy.logerr(
"Output directory is not empty, try adding -f if you want to overwrite it."
)
return
else:
output_path = prepare_unique_directory("%s_calib" % xtion.name)
# Initialise measurement
calib = CameraCalib(robot, tfb, xtion, output_path)
calib.confirm = not opt_no_confirm
# Run calibration capture
calib.run(trajs)
# Turn servo off
robot.set_servo_power_off()
import rospy
import tf2_ros
import numpy as np
import os
from clopema_libs import docopt
from clopema_libs.transform import frame2point
from clopema_calibration.table_calibration import TableCalibration
from clopema_calibration import write_frame_calibration
from clopema_robot import get_table_frames, get_xtion_val
if __name__ == '__main__':
rospy.init_node('table_calibration_check', anonymous=True)
args = docopt(__doc__, argv=rospy.myargv()[1:])
if args['--xtion'] is not None:
n = int(args['--xtion'])
image_topic_name = get_xtion_val(n, 'rgb_image')
camera_info_topic_name = get_xtion_val(n, 'rgb_info')
depth_image_topic_name = get_xtion_val(n, 'depth_image')
else:
image_topic_name = args['--image']
camera_info_topic_name = args['--camera']
tfb = tf2_ros.Buffer(cache_time=rospy.Duration(5.0))
tfl = tf2_ros.TransformListener(tfb)
rospy.sleep(1) # Wait for cache to fill up
frame_ids = get_table_frames(int(args['--table']))
import rospy
from clopema_libs import docopt
from clopema_libs.io import read_msgs
from clopema_libs.ui import ask
from clopema_robot import ClopemaRobotCommander
from moveit_msgs.msg import RobotTrajectory
_NODE_NAME = "execute_trajectory_cmd"
_DEBUG = False
if __name__ == "__main__":
# Parse command line
args = docopt(__doc__)
_DEBUG = args['--debug']
# Print all arguments if debug
if _DEBUG:
print args
# Initialize node
rospy.init_node(_NODE_NAME, anonymous=True)
# Initialize robot
robot = ClopemaRobotCommander("arms")
trajs = read_msgs(args['<file_name>'], RobotTrajectory)
# Check speed
-f, --sof Stop servo at the end.
--no-check Do not check collisions.
--eef-step STEP Step of the end effector [default: 0.01].
The '--' is required if you are using negative numbers.
"""
import rospy
from clopema_libs import docopt
from clopema_libs.ui import ask
from clopema_robot import ClopemaRobotCommander
from copy import deepcopy
if __name__ == '__main__':
rospy.init_node("move_pose", anonymous=True)
args = docopt(__doc__, options_first=True)
group = ClopemaRobotCommander.get_group_for_ee(args['--link'])
robot = ClopemaRobotCommander(group)
link_id = args['--link']
link = robot.robot.get_link(link_id)
pose = link.pose().pose
pose_ = deepcopy(pose)
eef_step = float(args['--eef-step'])
# Print info
print "group: ", group
print "link_id: ", link_id
print "eef_step: ", eef_step
if args['X'] is not None and args['X'] is not '-':
def main():
# Process command line arguments
argv = rospy.myargv(argv=sys.argv)
opt = docopt(__doc__, argv = argv[1:])
opt_debug = opt['--debug']
opt_xtion = int(opt['--xtion'])
opt_target_frame = opt['--frame']
opt_output = opt['<output_file>']
if opt_xtion == 1:
opt_other_s_link = "r2_joint_s"
opt_other_s_val = pi / 2
opt_ee_link = "xtion1_link_ee"
opt_group = "r1_xtion"
elif opt_xtion == 2:
opt_other_s_link = "r1_joint_s"
opt_other_s_val = -pi / 2
opt_ee_link = "xtion1_link_ee"
opt_group = "r2_xtion"
else:
rospy.logerr("Xtion number is not valid: %d", opt_xtion)
return
if opt_debug:
print opt
# Initialise node
rospy.init_node(NODE_NAME, anonymous=NODE_ANONYMOUS)
# Initialise tf2 buffer
tfb = get_tf2_buffer()
# Initialise robot commander
robot = ClopemaRobotCommander(opt_group)
# Get start position, all zeros, but exte rotated towards calibration table,
# and S of the other arm rotated -90 degrees.
current_state = robot.get_current_state()
start_state = deepcopy(current_state)
start_state.joint_state.position = [0] * len(start_state.joint_state.name)
start_state.joint_state.position[start_state.joint_state.name.index(opt_other_s_link)] = opt_other_s_val
start_state.joint_state.position[start_state.joint_state.name.index("ext_axis")] = base_rotation("base_link", opt_target_frame, tfb)
if opt_debug:
print start_state
# Generate poses
header = Header()
header.frame_id = opt_target_frame
pose_list = []
for x,y,z in xyzrange(L1_X_RANGE, L1_Y_RANGE, L1_Z_RANGE, L1_X_STEP, L1_Y_STEP, L1_Z_STEP):
p = make_pose(x, y, z, 0, 1, 0, 0)
pose_list.append(PoseStamped(header, p))
pose_list.append(PoseStamped(header, orient_pose(p)))
for x,y,z in xyzrange(L2_X_RANGE, L2_Y_RANGE, L2_Z_RANGE, L2_X_STEP, L2_Y_STEP, L2_Z_STEP):
p = make_pose(x, y, z, 0, 1, 0, 0)
pose_list.append(PoseStamped(header, p))
pose_list.append(PoseStamped(header, orient_pose(p)))
# TODO: Sort poses
# Plan throught poses
trajs = plan_through_poses(robot, start_state, pose_list, opt_ee_link)
# Print info
print "# generated poses: %d" % len(pose_list)
print "# trajectories: %d" % len(trajs)
# Wite trajectories
write_msgs(trajs, opt_output)
