def execute(self,parameter_name,blocking=False):
ah = ActionHandle("set", "light", parameter_name, blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="topic")
rospy.loginfo("Set light to <<%s>>",parameter_name)
# get joint values from parameter server
if type(parameter_name) is str:
if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
return 2
param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
else:
param = parameter_name
# check color parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for light: not a list, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
if not len(param) == 3: # check dimension
rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
print "parameter is:",param
ah.error_code = 3
return ah
else:
for i in param:
#print i,"type1 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
rospy.logdebug("accepted parameter %f for light",i)
# convert to ColorRGBA message
color = ColorRGBA()
color.r = param[0]
color.g = param[1]
color.b = param[2]
color.a = 1 # Transparency
# publish color
self.pub_light.publish(color)
ah.set_succeeded()
ah.error_code = 0
return ah
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking)
rospy.loginfo("Move <<%s>> PARAMETER to <<%s>>", component_name, target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
ah.set_failed(3)
return ah
return self.actions.move_arm_joint_direct(component_name, param, blocking)
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target,
blocking)
rospy.loginfo("Move <<%s>> PARAMETER to <<%s>>", component_name,
target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" +
component_name + "/" + target):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.ns_global_prefix + "/" + component_name + "/" +
target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" +
component_name + "/" + target)
else:
ah.set_failed(3)
return ah
return self.actions.move_arm_joint_direct(component_name, param,
blocking)
def execute(self, component_name, target=PoseStamped(), blocking=True):
ah = ActionHandle("move_arm_cartesian_planned", component_name, target, blocking)
rospy.loginfo("Move <<%s>> CARTESIAN PLANNED", component_name)
mp = MotionPlan()
mp += MoveArm("arm",[target,['sdh_grasp_link']])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(4)
break
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
def execute(self, target, blocking=True):
component_name = "base"
ah = ActionHandle("move_base", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
#get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
#check pose
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 3
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
#print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)):
#check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
#convert to pose message
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "/map"
pose.pose.position.x = param[0]
pose.pose.position.y = param[1]
pose.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, param[2])
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
action_server_name = "/move_base"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
#trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
#error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
# sending goal
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def execute(self, target="", blocking=True):
component_name = "gripper"
ah = ActionHandle("move_gripper", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.action_server_name_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.action_server_name_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.action_server_name_prefix + "/" + component_name + "/" + target)
else:
param = target
# check pose
"""
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
print i,"type1 = ", type(i)
DOF = 2
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
"""
pose_goal = MoveToJointConfigurationGoal()
DOF = 2
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.gripper1_joint_names[i]
jv.value = param[i]
jv.unit = "m"
pose_goal.goal.positions.append(jv)
action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveToJointConfigurationAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
print pose_goal
client.send_goal(pose_goal)
ah.set_client(client)
ah.wait_inside()
# it should block but it does not, so we wait!
rospy.sleep(3.0)
return ah
def execute(self, component_name, target=[0, 0, 0, "/base_link"], blocking=True):
ah = ActionHandle("move_arm_cart_sample_rpy_direct", component_name, target, blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
if not len(target) == 4 or not type(target[0])==float or not type(target[1])==float or not type(target[2])==float or not type(target[3]) == str: # checking parameters
rospy.logerr("no valid target parameters for %s: %s; aborting...", component_name, target)
ah.set_failed(3)
return ah
# convert to pose message
pose = MoveToCartesianPoseGoal()
pose.goal.header.stamp = rospy.Time.now()
pose.goal.header.frame_id = target[3]
pose.goal.pose.position.x = target[0]
pose.goal.pose.position.y = target[1]
pose.goal.pose.position.z = target[2]
(qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(0,0,0)
pose.goal.pose.orientation.x = qx
pose.goal.pose.orientation.y = qy
pose.goal.pose.orientation.z = qz
pose.goal.pose.orientation.w = qw
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
self.client.send_goal(pose)
ah.set_client(self.client)
ah.wait_inside()
return ah
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 5
if not len(target) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target))
print "parameter is:", target
ah.set_failed(3)
return ah
else:
for i in target:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
pose_goal = MoveToJointConfigurationGoal()
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.arm1_joint_names[i]
jv.value = target[i]
jv.unit = "rad"
pose_goal.goal.positions.append(jv)
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
#print client_goal
self.client.send_goal(pose_goal)
ah.set_client(self.client)
ah.wait_inside()
return ah
def execute(self,
component_name,
target=[0, 0, 0, "/base_link"],
blocking=True):
ah = ActionHandle("move_arm_cart_sample_rpy_direct", component_name,
target, blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
if not len(target) == 4 or not type(
target[0]) == float or not type(
target[1]) == float or not type(
target[2]) == float or not type(
target[3]) == str: # checking parameters
rospy.logerr(
"no valid target parameters for %s: %s; aborting...",
component_name, target)
ah.set_failed(3)
return ah
# convert to pose message
pose = MoveToCartesianPoseGoal()
pose.goal.header.stamp = rospy.Time.now()
pose.goal.header.frame_id = target[3]
pose.goal.pose.position.x = target[0]
pose.goal.pose.position.y = target[1]
pose.goal.pose.position.z = target[2]
(qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(0, 0, 0)
pose.goal.pose.orientation.x = qx
pose.goal.pose.orientation.y = qy
pose.goal.pose.orientation.z = qz
pose.goal.pose.orientation.w = qw
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",
self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr(
"%s action server not ready within timeout, aborting...",
self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
self.client.send_goal(pose)
ah.set_client(self.client)
ah.wait_inside()
return ah
def execute(self, component_name, as_name, target="", blocking=True):
ah = ActionHandle("move_joint_trajectory", component_name, target,
blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get joint_names from parameter server
param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
if not rospy.has_param(param_string):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
param_string)
ah.set_failed(2)
return ah
joint_names = rospy.get_param(param_string)
# check joint_names parameter
if not type(joint_names) is list: # check list
rospy.logerr(
"no valid joint_names for %s: not a list, aborting...",
component_name)
print "joint_names are:", joint_names
ah.set_failed(3)
return ah
else:
for i in joint_names:
#print i,"type1 = ", type(i)
if not type(i) is str: # check string
rospy.logerr(
"no valid joint_names for %s: not a list of strings, aborting...",
component_name)
print "joint_names are:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted joint_names for component %s",
component_name)
# get joint values from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" +
component_name + "/" + target):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.ns_global_prefix + "/" + component_name + "/" +
target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" +
component_name + "/" + target)
else:
param = target
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
traj = []
for point in param:
#print point,"type1 = ", type(point)
if type(point) is str:
if not rospy.has_param(self.ns_global_prefix + "/" +
component_name + "/" + point):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.ns_global_prefix + "/" + component_name + "/" +
point)
ah.set_failed(2)
return ah
point = rospy.get_param(self.ns_global_prefix + "/" +
component_name + "/" + point)
point = point[
0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print point
elif type(point) is list:
rospy.logdebug("point is a list")
else:
rospy.logerr(
"no valid parameter for %s: not a list of lists or strings, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
# here: point should be list of floats/ints
#print point
if not len(point) == len(joint_names): # check dimension
rospy.logerr(
"no valid parameter for %s: dimension should be %d and is %d, aborting...",
component_name, len(joint_names), len(point))
print "parameter is:", param
ah.set_failed(3)
return ah
for value in point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or
(type(value) is int)): # check type
#print type(value)
rospy.logerr(
"no valid parameter for %s: not a list of float or int, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
rospy.logdebug("accepted value %f for %s", value,
component_name)
traj.append(point)
rospy.logdebug("accepted trajectory for %s", component_name)
# convert to ROS trajectory message
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
traj_msg.joint_names = joint_names
point_nr = 0
for point in traj:
point_nr = point_nr + 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0] * len(joint_names)
point_msg.time_from_start = rospy.Duration(
3 * point_nr
) # this value is set to 3 sec per point. \todo TODO: read from parameter
traj_msg.points.append(point_msg)
# call action server
action_server_name = "/" + component_name + '_controller/follow_joint_trajectory'
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name,
FollowJointTrajectoryAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",
action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr(
"%s action server not ready within timeout, aborting...",
action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
# sending goal
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
#print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def execute(self, component_name, as_name, target="", blocking=True):
ah = ActionHandle("move_joint_trajectory", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>",component_name,target)
# get joint_names from parameter server
param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
if not rospy.has_param(param_string):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
ah.set_failed(2)
return ah
joint_names = rospy.get_param(param_string)
# check joint_names parameter
if not type(joint_names) is list: # check list
rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
print "joint_names are:",joint_names
ah.set_failed(3)
return ah
else:
for i in joint_names:
#print i,"type1 = ", type(i)
if not type(i) is str: # check string
rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
print "joint_names are:",param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted joint_names for component %s",component_name)
# get joint values from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
traj = []
for point in param:
#print point,"type1 = ", type(point)
if type(point) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + point):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + point)
ah.set_failed(2)
return ah
point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + point)
point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print point
elif type(point) is list:
rospy.logdebug("point is a list")
else:
rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
# here: point should be list of floats/ints
#print point
if not len(point) == len(joint_names): # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
print "parameter is:",param
ah.set_failed(3)
return ah
for value in point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or (type(value) is int)): # check type
#print type(value)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
rospy.logdebug("accepted value %f for %s",value,component_name)
traj.append(point)
rospy.logdebug("accepted trajectory for %s",component_name)
# convert to ROS trajectory message
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now()+rospy.Duration(0.5)
traj_msg.joint_names = joint_names
point_nr = 0
for point in traj:
point_nr = point_nr + 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0]*len(joint_names)
point_msg.time_from_start=rospy.Duration(3*point_nr) # this value is set to 3 sec per point. \todo TODO: read from parameter
traj_msg.points.append(point_msg)
# call action server
action_server_name = "/" + component_name + '_controller/follow_joint_trajectory'
rospy.logdebug("calling %s action server",action_server_name)
client = actionlib.SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready",action_server_name)
# sending goal
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
#print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def execute(self, target=PoseStamped(), blocking=True):
ah = ActionHandle("pick_up", "dummy", "", blocking)
rospy.loginfo("Picking up object...")
#ah = self.actions.grasp_object(target, blocking)
wi = WorldInterface()
wi.reset_attached_objects()
wi.reset_collision_objects()
# add table
table_extent = (2.0, 2.0, 1.0)
table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link')
wi.add_collision_box(table_pose, table_extent, "table")
mp = MotionPlan()
mp += CallFunction(sss.move, 'torso','front')
#mp += MoveArm('arm',['pregrasp'])
mp += CallFunction(sss.move, 'sdh','cylopen', False)
# OpenIssues:
# - where to place the sdh_grasp_link to grasp the object located at target?
# - there is no orientation in the target? -> hardcoded taken from pregrasp
grasp_pose = PoseStamped()
grasp_pose = deepcopy(target)
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.orientation.x = 0.220
grasp_pose.pose.orientation.y = 0.670
grasp_pose.pose.orientation.z = -0.663
grasp_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
mp += CallFunction(sss.move, 'sdh','cylclosed', True)
#ah = self.actions.lift_object(target, blocking)
lift_pose = PoseStamped()
lift_pose = deepcopy(target)
lift_pose.header.stamp = rospy.Time.now()
lift_pose.pose.position.z += 0.08
lift_pose.pose.orientation.x = 0.220
lift_pose.pose.orientation.y = 0.670
lift_pose.pose.orientation.z = -0.663
lift_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
#ah = self.actions.retrieve_object(target, blocking)
#mp += MoveArm('arm',['pregrasp'])
mp += MoveArm('arm',['wavein'])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(4)
break
ah.set_succeeded()
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target,
blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",
component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 5
if not len(target) == DOF: # check dimension
rospy.logerr(
"no valid parameter for %s: dimension should be %d and is %d, aborting...",
component_name, DOF, len(target))
print "parameter is:", target
ah.set_failed(3)
return ah
else:
for i in target:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or
(type(i) is int)): # check type
#print type(i)
rospy.logerr(
"no valid parameter for %s: not a list of float or int, aborting...",
component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i,
component_name)
pose_goal = MoveToJointConfigurationGoal()
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.arm1_joint_names[i]
jv.value = target[i]
jv.unit = "rad"
pose_goal.goal.positions.append(jv)
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",
self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr(
"%s action server not ready within timeout, aborting...",
self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
#print client_goal
self.client.send_goal(pose_goal)
ah.set_client(self.client)
ah.wait_inside()
return ah
def execute(self, component_name, parameter_name=[0,0,0], blocking=True):
ah = ActionHandle("move_base_rel", component_name, parameter_name, blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="topic")
rospy.loginfo("Move base relatively by <<%s>>", parameter_name)
# step 0: check validity of parameters:
if not len(parameter_name) == 3 or not isinstance(parameter_name[0], (int, float)) or not isinstance(parameter_name[1], (int, float)) or not isinstance(parameter_name[2], (int, float)):
rospy.logerr("Non-numeric parameter_name list, aborting move_base_rel")
print("parameter_name must be numeric list of length 3; (relative x and y transl [m], relative rotation [rad])")
ah.set_failed(4)
return ah
if math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) >= 0.15:
rospy.logerr("Maximal relative translation step exceeded, aborting move_base_rel")
print("Maximal relative translation step is 0.1 m")
ah.set_failed(4)
return(ah)
if abs(parameter_name[2]) >= math.pi/2:
rospy.logerr("Maximal relative rotation step exceeded, aborting move_base_rel")
print("Maximal relative rotation step is pi/2")
ah.set_failed(4)
return(ah)
# step 1: determine duration of motion so that upper thresholds for both translational as well as rotational velocity are not exceeded
max_trans_vel = 0.05 # [m/s]
max_rot_vel = 0.2 # [rad/s]
duration_trans_sec = math.sqrt(parameter_name[0]**2 + parameter_name[1]**2) / max_trans_vel
duration_rot_sec = abs(parameter_name[2] / max_rot_vel)
duration_sec = max(duration_trans_sec, duration_rot_sec)
duration_ros = rospy.Duration.from_sec(duration_sec) # duration of motion in ROS time
# step 2: determine actual velocities based on calculated duration
x_vel = parameter_name[0] / duration_sec
y_vel = parameter_name[1] / duration_sec
rot_vel = parameter_name[2] / duration_sec
# step 3: send constant velocity command to base_controller for the calculated duration of motion
pub = rospy.Publisher('/base_controller/command_safe', Twist) # todo: use Matthias G.'s safe_command
twist = Twist()
twist.linear.x = x_vel
twist.linear.y = y_vel
twist.angular.z = rot_vel
r = rospy.Rate(10) # send velocity commands at 10 Hz
end_time = rospy.Time.now() + duration_ros
while not rospy.is_shutdown() and rospy.Time.now() < end_time:
pub.publish(twist)
r.sleep()
ah.set_succeeded()
return ah
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_planned", component_name, target, blocking)
rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,target)
# get joint values from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
last_point = param[-1]
if type(last_point) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + last_point):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + last_point)
ah.set_failed(2)
return ah
last_point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + last_point)
last_point = last_point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print last_point
elif type(last_point) is list:
rospy.logdebug("last_point is a list")
else:
rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
# here: last_point should be list of floats/ints
#print last_point
if not len(last_point) == self.DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,self.DOF,len(last_point))
print "parameter is:",param
ah.set_failed(3)
return ah
for value in last_point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or (type(value) is int)): # check type
#print type(value)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
rospy.logdebug("accepted value %f for %s",value,component_name)
mp = MotionPlan()
mp += MoveArm("arm", [last_point])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(3)
break
else:
rospy.logerr("Planning failed")
ah.set_failed(3)
return ah
def execute(self, parameter_name, blocking=True, mode=''):
component_name = 'base'
ah = ActionHandle("move", component_name, parameter_name, blocking)
if(mode == None or mode == ""):
rospy.loginfo("Move <<%s>> to <<%s>>",component_name,parameter_name)
else:
rospy.loginfo("Move <<%s>> to <<%s>> using <<%s>> mode",component_name,parameter_name,mode)
# get joint values from parameter server
if type(parameter_name) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + parameter_name)
else:
param = parameter_name
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 3
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,DOF,len(param))
print "parameter is:",param
ah.set_failed(3)
return ah
else:
for i in param:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s",i,component_name)
# convert to pose message
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "/map"
pose.pose.position.x = param[0]
pose.pose.position.y = param[1]
pose.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, param[2])
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
# call action server
if(mode == None or mode == ""):
action_server_name = "/move_base"
elif(mode == "omni"):
action_server_name = "/move_base"
elif(mode == "diff"):
action_server_name = "/move_base_diff"
elif(mode == "linear"):
action_server_name = "/move_base_linear"
else:
rospy.logerr("no valid navigation mode given for %s, aborting...",component_name)
print "navigation mode is:",mode
ah.set_failed(33)
return ah
rospy.logdebug("calling %s action server",action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready",action_server_name)
# sending goal
client_goal = MoveBaseGoal()
client_goal.target_pose = pose
#print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def execute(self, target="", blocking=True):
component_name = "gripper"
ah = ActionHandle("move_gripper", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.action_server_name_prefix + "/" +
component_name + "/" + target):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.action_server_name_prefix + "/" + component_name +
"/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.action_server_name_prefix + "/" +
component_name + "/" + target)
else:
param = target
# check pose
"""
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
print i,"type1 = ", type(i)
DOF = 2
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
"""
pose_goal = MoveToJointConfigurationGoal()
DOF = 2
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.gripper1_joint_names[i]
jv.value = param[i]
jv.unit = "m"
pose_goal.goal.positions.append(jv)
action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name,
MoveToJointConfigurationAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",
action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr(
"%s action server not ready within timeout, aborting...",
action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
print pose_goal
client.send_goal(pose_goal)
ah.set_client(client)
ah.wait_inside()
# it should block but it does not, so we wait!
rospy.sleep(3.0)
return ah
