def subscriber_callback(data):
occupancyMap = transformation(data.data, data.info.width, data.info.height)
way_points = find_goals(occupancyMap, data.info.width, data.info.height)
result = random.choice(way_points)
try:
planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
listener = TransformListener()
listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
t = listener.getLatestCommonTime("base_link", "map")
position, quaternion = listener.lookupTransform("base_link", "map", t)
pos_x = position[0]
pos_y = position[1]
pos_z = position[2]
goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
start_pose = create_message(pos_x,pos_y,pos_z,quaternion)
plan = planMaker(
start_pose,
goal_robot.target_pose,
0.5)
action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
action_server.wait_for_server()
send_goal(goal_robot, action_server)
except rospy.ServiceException, e:
print "plan service call failed: %s"%e
class FaceCommander(Head):
def __init__(self):
Head.__init__(self)
self._world = 'base'
self._tf_listener = TransformListener()
self.set_pan(0)
def look_at(self, obj=None):
if obj is None:
self.set_pan(0)
return True
if isinstance(obj, str):
pose = self._tf_listener.lookupTransform('head', obj, rospy.Time(0))
else:
rospy.logerr("FaceCommander.look_at() accepts only strings atm")
return False
hyp = transformations.norm(pose)
adj = pose[0][1]
angle = pi/2 - arccos(float(adj)/hyp)
if isnan(angle):
rospy.logerr('FaceCommander cannot look at {}'.format(obj))
return False
self.set_pan(angle)
return True
class ChallengeProblemLogger(object):
_knownObstacles = {}
_placedObstacle = False
_lastgzlog = 0.0
_tf_listener = None
def __init__(self,name):
self._name = name;
self._experiment_started = False
self._tf_listener = TransformListener()
# Subscribe to robot pose ground truth from gazebo
rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.gazebo_model_monitor,
queue_size=1)
# Whenever we get a report from Gazebo, map the gazebo coordinate to map coordinates and
# log this
# Only do this every second - this should be accurate enough
# TODO: model is assumed to be the third in the list. Need to make this based
# on the array to account for obstacles (maybe)
def gazebo_model_monitor(self, data):
if (len(data.pose))<=2:
return
data_time = rospy.get_rostime().to_sec()
if ((self._lastgzlog == 0.0) | (data_time - self._lastgzlog >= 1)):
# Only do this every second
# Get the turtlebot model state information (assumed to be indexed at 2)
tb_pose = data.pose[2]
tb_position = tb_pose.position
self._lastgzlog = data_time
# Do this only if the transform exists
if self._tf_listener.frameExists("/base_link") and self._tf_listener.frameExists("/map"):
self._tf_listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(1))
(trans,rot) = self._tf_listener.lookupTransform("/map", "/base_link",rospy.Time(0))
rospy.loginfo("BRASS | Turtlebot | {},{}".format(trans[0], trans[1]))
# Log any obstacle information, but do it only once. This currently assumes one obstacle
# TODO: test this
if len(data.name) > 3:
addedObstacles = {}
removedObstacles = self._knownObstacles.copy()
for obs in range(3, len(data.name)-1):
if (data.name[obs] not in self._knownObstacles):
addedObstacles[data.name[obs]] = obs
else:
self._knownObstacles[data.name[obs]] = obs
del removedObstacles[data.name[obs]]
for key, value in removedObstacles.iteritems():
rospy.logInfo("BRASS | Obstacle {} | removed".format(key))
del self._knownObstacles[key]
for key, value in addedObstacles.iteritems():
obs_pos = data.pose[value].position
rospy.logInfo ("BRASS | Obstacle {} | {},{}".format(key, obs_pos.x, obs_pos.y))
self._knownObstacles[key] = value
def calculate_distance_to_rows():
tflisten = TransformListener()
dist = []
for i in range(0,n_rows):
(veh_trans,veh_rot) = tflisten.lookupTransform("odom","base_footprint",rospy.Time(0))
pass
class TfFilter:
def __init__(self, buffer_size):
self.tf = TransformListener(True, rospy.Duration(5))
self.target = ''
self.buffer = np.zeros((buffer_size, 1))
self.buffer_ptr = 0
self.buffer_size = buffer_size
self.tf_sub = rospy.Subscriber('tf', tfMessage, self.tf_cb)
self.tf_pub = rospy.Publisher('tf', tfMessage)
self.srv = rospy.Service(SERVICE, PublishGoal, self.publish_goal_cb)
def tf_cb(self, msg):
for t in msg.transforms:
if t.child_frame_id == self.target:
time = self.tf.getLatestCommonTime(self.source, self.target)
p, q = self.tf.lookupTransform(self.source, self.target, time)
rm = tfs.quaternion_matrix(q)
# assemble a new coordinate frame that has z-axis aligned with
# the vertical direction and x-axis facing the z-axis of the
# original frame
z = rm[:3, 2]
z[2] = 0
axis_x = tfs.unit_vector(z)
axis_z = tfs.unit_vector([0, 0, 1])
axis_y = np.cross(axis_x, axis_z)
rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
# shift the pose along the x-axis
self.position = p + np.dot(rm, self.d)[:3]
self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2]
self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size
self.publish_filtered_tf(t.header)
def publish_filtered_tf(self, header):
yaw = np.median(self.buffer)
q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
ts = TransformStamped()
ts.header = header
ts.header.frame_id = self.source
ts.child_frame_id = self.goal
ts.transform.translation.x = self.position[0]
ts.transform.translation.y = self.position[1]
ts.transform.translation.z = 0
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
msg = tfMessage()
msg.transforms.append(ts)
self.tf_pub.publish(msg)
def publish_goal_cb(self, r):
rospy.loginfo('Received [%s] request. Will start publishing %s frame' %
(SERVICE, r.goal_frame_id))
self.source = r.source_frame_id
self.target = r.target_frame_id
self.goal = r.goal_frame_id
self.d = [r.displacement.x, r.displacement.y, r.displacement.z]
return []
class KinectNiteHelp:
def __init__(self, name="kinect_listener", user=1):
# rospy.init_node(name, anonymous=True)
self.tf = TransformListener()
self.user = user
def getLeftHandPos(self):
if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"):
print " Inside TF IF Get LEft HAnd POS "
t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1")
(leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t)
return leftHandPos
def getRightHandPos(self):
if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"):
t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1")
(rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t)
return rightHandPos
class TransformNode:
def __init__(self, *args):
self.tf = TransformListener()
rospy.Subscriber("/tf", TFMessage, transform, queue_size=1)
def transform(self, msg):
if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
t = self.tf.getLatestCommonTime("/base_link", "/map")
position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
print position, quaternion
class TwistToPoseConverter(object):
def __init__(self):
self.ee_frame = rospy.get_param('~ee_frame', "/l_gripper_tool_frame")
self.twist_sub = rospy.Subscriber('/twist_in', TwistStamped, self.twist_cb)
self.pose_pub = rospy.Publisher('/pose_out', PoseStamped)
self.tf_listener = TransformListener()
def get_ee_pose(self, frame='/torso_lift_link', time=None):
"""Get current end effector pose from tf."""
try:
now = rospy.Time.now() if time is None else time
self.tf_listener.waitForTransform(frame, self.ee_frame, now, rospy.Duration(4.0))
pos, quat = self.tf_listener.lookupTransform(frame, self.ee_frame, now)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[%s] TF Failure getting current end-effector pose:\r\n %s"
%(rospy.get_name(), e))
return None, None
return pos, quat
def twist_cb(self, ts):
"""Get current end effector pose and augment with twist command."""
cur_pos, cur_quat = self.get_ee_pose(ts.header.frame_id)
ps = PoseStamped()
ps.header.frame_id = ts.header.frame_id
ps.header.stamp = ts.header.stamp
ps.pose.position.x = cur_pos[0] + ts.twist.linear.x
ps.pose.position.y = cur_pos[1] + ts.twist.linear.y
ps.pose.position.z = cur_pos[2] + ts.twist.linear.z
twist_quat = trans.quaternion_from_euler(ts.twist.angular.x,
ts.twist.angular.y,
ts.twist.angular.z)
final_quat = trans.quaternion_multiply(twist_quat, cur_quat)
ps.pose.orientation = Quaternion(*final_quat)
try:
self.tf_listener.waitForTransform('/torso_lift_link', ps.header.frame_id,
ps.header.stamp, rospy.Duration(3.0))
pose_out = self.tf_listener.transformPose('/torso_lift_link', ps)
self.pose_pub.publish(pose_out)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[%s] TF Failure transforming goal pose to torso_lift_link:\r\n %s"
%(rospy.get_name(), e))
class Transformation():
def __init__(self):
pub = 0
self.tf = TransformListener()
self.tf1 = TransformerROS()
self.fdata = geometry_msgs.msg.TransformStamped()
self.fdata_base = geometry_msgs.msg.TransformStamped()
self.transform = tf.TransformBroadcaster()
self.wrench = WrenchStamped()
self.wrench_bl = WrenchStamped()
def wrench_cb(self,msg):
self.wrench = msg.wrench
self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
self.fdata.transform.translation.x = self.wrench.force.x
self.fdata.transform.translation.y = self.wrench.force.y
self.fdata.transform.translation.z = self.wrench.force.z
try:
if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
#print transform_ee_base_position
#print transform_ee_base_quaternion
#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
except(tf.LookupException,tf.ConnectivityException):
print("TRANSFORMATION ERROR")
sss.say(["error"])
#return 'failed'
self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))
self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))
self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
self.wrench_bl.header.stamp = rospy.Time.now();
self.pub.publish(self.wrench_bl)
class Demo:
def __init__(self, goals):
rospy.init_node("demo", anonymous=True)
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 0
def run(self):
self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = "world"
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime("/world", self.frame)
if self.listener.canTransform("/world", self.frame, t):
position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if (
math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
and self.goalIndex < len(self.goals) - 1
):
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
class getStepTowardsPoint(smach.State):
"""
Calculates the next step towards a point defined in userdata.
"""
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'], input_keys=['next_x','next_y'],output_keys=['step_next_x','step_next_y'])
self.__listen = TransformListener()
self.__cur_pos = None
self.__step_next_pos = None
self.__odom_frame = rospy.get_param("~odom_frame","/odom")
self.__base_frame = rospy.get_param("~base_frame","/base_footprint")
self.step = 1
def __get_current_position(self):
ret = False
try:
(self.__cur_pos,rot) = self.__listen.lookupTransform( self.__odom_frame,self.__base_frame,rospy.Time(0))
ret = True
except (tf.LookupException, tf.ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
return ret
class mbzirc_c2_auto():
# A few key tasks are achieved in the initializer function:
# 1. We load the pre-defined search routine
# 2. We connect to the move_base server in ROS
# 3. We start the ROS subscriber callback function registering the object
# 4. We initialize counters in the class to be shared by the various callback routines
def __init__(self):
# Name this node, it must be unique
rospy.init_node('autonomous', anonymous=True)
# Enable shutdown in rospy (This is important so we cancel any move_base goals
# when the node is killed)
rospy.on_shutdown(self.shutdown)
self.rest_time = rospy.get_param("~rest_time",
0.1) # Minimum pause at each location
self.stalled_threshold = rospy.get_param("~stalled_threshold",
100) # Loops before stall
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
self.waypoints = list()
self.tf = TransformListener()
# self.locations = dict()
# self.wpname = dict()
rospack = rospkg.RosPack()
f = open(
rospack.get_path('husky_wp') + '/params/pre-defined-standby.txt',
'r')
# ct2 = 0
with f as openfileobject:
first_line = f.readline()
for line in openfileobject:
nome = [x.strip() for x in line.split(',')]
#self.wpname[ct2] = nome[0]
x = Decimal(nome[1])
y = Decimal(nome[2])
z = Decimal(nome[3])
X = Decimal(nome[4])
Y = Decimal(nome[5])
Z = Decimal(nome[6])
#self.locations[self.wpname[ct2]] = Pose(Point(x,y,z), Quaternion(X,Y,Z,1))
self.waypoints.append(
Pose(Point(x, y, z), Quaternion(0, 0, 0, 1)))
#print self.waypoints
#time.sleep(1)
# ct2 = ct2+1
#self.wp = -1
#self.ct4 = 0
print "Static path has : "
print len(self.waypoints)
print " point(s)."
time.sleep(5)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < len(self.waypoints) and not rospy.is_shutdown():
# Update the marker display
#self.marker_pub.publish(self.markers)
time.sleep(2)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = self.waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
# check if the goal point is found by the detect_goal node
if rospy.has_param('/panel_goal'):
# go to goal the goal goint
# get parameter
panel_goal = rospy.get_param("/panel_goal")
print panel_goal[0]
print panel_goal[1]
print panel_goal[2]
while not rospy.is_shutdown():
try:
# Find the global coordinate of the UGV
(trans, rot) = self.tf.lookupTransform(
'/odom', '/base_link', rospy.Time(0))
# Calculate the vector between the UGV and the goal point
Tx = panel_goal[0] - trans[0]
Ty = panel_goal[1] - trans[1]
print "Vehicle global coordinates is:"
print trans[0]
print trans[1]
print "The travel vecor is:"
print Tx
print Ty
# Calculate the travel distance between the UGV and the goal point
travel_distance = math.sqrt(
math.pow(Tx, 2) + math.pow(Ty, 2))
print "The travel distance is:"
print travel_distance
# Calculate a scaling vector to make the UGV stop at 1.5m away from the goal.
travel_distance2 = travel_distance - 1.5
factor = travel_distance2 / travel_distance
print "The scaliing factor is:"
print factor
Tx = Tx * factor
Ty = Ty * factor
# Calulate the goal point in the global frame
goal_x = trans[0] + Tx
goal_y = trans[1] + Ty
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
print "The goal is:"
print goal_x
print goal_y
goal.target_pose.pose = Pose(Point(goal_x, goal_y, 0),
Quaternion(0, 0, 0, 1))
self.move(goal)
rospy.loginfo("Reach goal")
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("No transformation")
break
else:
rospy.loginfo("No goal found")
def move(self, goal):
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(600))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class Pick(ScenarioStateBase):
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'pickup',
save_sm_state=save_sm_state,
output_keys=['grasped_object'],
outcomes=[
'succeeded', 'failed',
'failed_after_retrying',
'find_objects_before_picking'
])
self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects')
self.state_name = kwargs.get('state_name', 'pick')
self.timeout = kwargs.get('timeout', 120.)
self.tf_listener = TransformListener()
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
def execute(self, userdata):
if self.save_sm_state:
self.save_current_state()
surface_objects = self.get_surface_objects(surface_prefix='table')
object_poses = self.get_object_poses(surface_objects)
surface, obj_to_grasp_idx = self.select_object_for_grasping(
object_poses)
obj_to_grasp = ''
if obj_to_grasp_idx != -1:
obj_to_grasp = surface_objects[surface][obj_to_grasp_idx]
else:
rospy.logerr('Could not find an object to grasp')
self.say('Could not find an object to grasp')
return 'find_objects_before_picking'
dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface)
rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface))
self.say('Picking ' + obj_to_grasp + ' from ' + surface)
self.action_dispatch_pub.publish(dispatch_msg)
self.executing = True
self.succeeded = False
start_time = time.time()
duration = 0.
while self.executing and duration < self.timeout:
rospy.sleep(0.1)
duration = time.time() - start_time
if self.succeeded:
rospy.loginfo('%s grasped successfully' % obj_to_grasp)
self.say('Successfully grasped ' + obj_to_grasp)
userdata.grasped_object = obj_to_grasp
return 'succeeded'
rospy.loginfo('Could not grasp %s' % obj_to_grasp)
self.say('Could not grasp ' + obj_to_grasp)
if self.retry_count == self.number_of_retries:
rospy.loginfo('Failed to grasp %s' % obj_to_grasp)
self.say('Aborting operation')
return 'failed_after_retrying'
rospy.loginfo('Retrying to grasp %s' % obj_to_grasp)
self.retry_count += 1
return 'failed'
def get_surface_objects(self, surface_prefix='table'):
surface_objects = dict()
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'on'
result = self.attribute_fetching_client(request)
for item in result.attributes:
object_on_desired_surface = False
object_name = ''
surface_name = ''
if not item.is_negative:
for param in item.values:
if param.key == 'plane' and param.value.find(
surface_prefix) != -1:
object_on_desired_surface = True
surface_name = param.value
if surface_name not in surface_objects:
surface_objects[surface_name] = list()
elif param.key == 'obj':
object_name = param.value
if object_on_desired_surface:
surface_objects[surface_name].append(object_name)
return surface_objects
def get_object_poses(self, surface_objects):
object_poses = dict()
for surface, objects in surface_objects.items():
object_poses[surface] = list()
for obj_name in objects:
try:
obj = self.msg_store_client.query_named(
obj_name, Object._type)[0]
object_poses[surface].append(obj.pose)
except:
rospy.logerr('Error retriving knowledge about %s',
obj_name)
pass
return object_poses
def select_object_for_grasping(self, object_poses):
'''Returns the index of the object whose distance is closest to the robot
'''
object_surfaces = list()
distances = dict()
robot_position = np.zeros(3)
for surface, poses in object_poses.items():
object_surfaces.append(surface)
distances[surface] = list()
for pose in poses:
base_link_pose = self.tf_listener.transformPose(
'base_link', pose)
distances[surface].append(
self.distance(
robot_position,
np.array([
base_link_pose.pose.position.x,
base_link_pose.pose.position.y,
base_link_pose.pose.position.z
])))
min_dist = 1e100
min_dist_obj_idx = -1
obj_surface = ''
for surface, distance_list in distances.items():
if distance_list:
surface_min_dist = np.min(distance_list)
if surface_min_dist < min_dist:
min_dist = surface_min_dist
min_dist_obj_idx = np.argmin(distance_list)
obj_surface = surface
return obj_surface, min_dist_obj_idx
def get_robot_pose(self, map_frame='map', base_link_frame='base_link'):
latest_tf_time = self.tf_listener.getLatestCommonTime(
base_link_frame, map_frame)
position, quat_orientation = self.tf_listener.lookupTransform(
base_link_frame, map_frame, latest_tf_time)
return position, quat_orientation
def distance(self, point1, point2):
return np.linalg.norm(np.array(point1) - np.array(point2))
def get_dispatch_msg(self, obj_name, surface_name):
dispatch_msg = plan_dispatch_msgs.ActionDispatch()
dispatch_msg.name = self.action_name
arg_msg = diag_msgs.KeyValue()
arg_msg.key = 'bot'
arg_msg.value = self.robot_name
dispatch_msg.parameters.append(arg_msg)
arg_msg = diag_msgs.KeyValue()
arg_msg.key = 'obj'
arg_msg.value = obj_name
dispatch_msg.parameters.append(arg_msg)
arg_msg = diag_msgs.KeyValue()
arg_msg.key = 'plane'
arg_msg.value = surface_name
dispatch_msg.parameters.append(arg_msg)
return dispatch_msg
class FindObstacles(object):
# we increment self.id at every complete scan.
# per complete scan, we publish (potentially) multiple PolygonStamped
# msgs to /obstacles, all with the same header.seq value (self.id)
def __init__(self, viz=True, experiment=None):
self.id = 1
rospy.init_node('obstacles_node')
self.tf = TransformListener()
self.num_obj = 11
pub = rospy.Publisher('/obstacles', Polygons, queue_size=50)
obstacle_viz = rospy.Publisher('/obstacles_viz',
MarkerArray,
queue_size=50)
if experiment:
if experiment == "-b":
#print("INIT EXPERIMENT")
goal_pub = rospy.Publisher('/goal_state',
Point32,
queue_size=10)
human_obs_pub = rospy.Publisher('obstacle2_poly',
PolygonStamped,
queue_size=1)
human_v_pub = rospy.Publisher('obstacle2_v',
Twist,
queue_size=1)
self.num_obj = 1
if experiment == '-6':
#print("INIT EXPERIMENT")
human_pub = rospy.Publisher('/human', Point32, queue_size=10)
human_obs_pub = rospy.Publisher('obstacle2_poly',
PolygonStamped,
queue_size=1)
human_v_pub = rospy.Publisher('obstacle2_v',
Twist,
queue_size=1)
self.num_obj = 1
if experiment == '-g':
#print("MODE SWITCHING ENABLED")
goal_pub = rospy.Publisher('/goal_state',
Point32,
queue_size=10)
human_pub = rospy.Publisher('/human', Point32, queue_size=10)
human_obs_pub = rospy.Publisher('obstacle2_poly',
Polygons,
queue_size=1)
human_v_pub = rospy.Publisher('obstacle2_v',
Twist,
queue_size=1)
self.num_obj = 3
# keep track of real lines from the last frame
self.prev_real_lines = set()
# keep track of img and lines from the last frame
self.latest_img, self.latest_lines = np.zeros((GRID, GRID)), []
self.x = [0, 0, 0]
self.prev = None
self.prevHuman = None
# MODE 0: Goal Following, 1: Assistant
self.mode = 0
def odom_callback(msg):
dd, asd, yaw = euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
self.x = [msg.pose.pose.position.x, msg.pose.pose.position.y, yaw]
##print("turtlebot ODOM", self.x)
def mode_callback(msg):
self.mode = msg.data
rospy.Subscriber('/odom', Odometry, odom_callback)
rospy.Subscriber('/mode', UInt16, mode_callback)
def find_lines_and_pub(lidar_data):
#lines, img = find_lines(lidar_data, self.prev_real_lines, self.x)
# apply transformation
try:
pos, qu = self.tf.lookupTransform("/odom", "/base_scan",
rospy.Time())
##print("TRANS", pos, qu)
t_mat = self.tf.fromTranslationRotation(pos, qu)
n = len(lidar_data.points)
p_mat = np.ones((4, n))
for i in range(n):
pt = lidar_data.points[i]
p_mat[0, i] = pt.x
p_mat[1, i] = pt.y
p_mat[2, i] = 0
p_trans = np.dot(t_mat, p_mat)
except:
return
# process line data into usable form
lines = set()
for i in range(n / 2):
line = ((p_trans[0, 2 * i], p_trans[1, 2 * i]),
(p_trans[0, 2 * i + 1], p_trans[1, 2 * i + 1]))
lines.add(line)
polys, obs = make_polys(lines, pos)
# associates meas obs with previous obs
obs_fil, flag = data_association(obs, self.prev, self.num_obj)
pts = []
visual = MarkerArray()
ctr = 1
for o in obs_fil:
# inits kf if one doesn't exist for this obs
if o.kf == None:
#print("INITIALIZING KF")
o.kf = o.init_kalman()
# update kalman
meas = np.append(o.center, o.area)
o.mean, o.cov = o.kf.filter_update(o.mean,
o.cov,
observation=meas)
# update
o.center = [o.mean[0], o.mean[3]]
o.area = o.mean[6]
# visualization
add = Point()
add.x = o.mean[0]
add.y = o.mean[3]
pts.append(add)
visual.markers.append(o.get_marker_meas(ctr))
o.update_poly()
visual.markers.append(o.get_marker(ctr))
ctr += 1
marker = Marker()
marker.header.frame_id = "/odom"
marker.id = 0
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.b = 1.0
marker.color.a = 1.0
marker.points = pts
visual.markers.append(marker)
obstacle_viz.publish(visual)
# only allow "good" measurements to be the references
if len(obs_fil) == self.num_obj and not flag:
self.prev = obs_fil
# #print("Kalman est:", self.mean)
if experiment:
if experiment == "-b":
if len(obs_fil) == self.num_obj:
o = obs_fil[0]
cen = o.center_sq()
goal = Point32()
goal.x = cen[0]
goal.y = cen[1]
#goal_pub.publish(goal)
#human_pub.publish(o.poly)
#human_v_pub.publish(o.toVel())
if experiment == "-6":
if len(obs_fil) == self.num_obj:
o = obs_fil[0]
cen = o.center_sq()
goal = Point32()
goal.x = cen[0]
goal.y = cen[1]
human_pub.publish(goal)
human_obs_pub.publish(o.poly)
human_v_pub.publish(o.toVel())
if experiment == "-g":
if len(
obs_fil
) >= self.num_obj: # currently hard-coded to assume num_obj objects
polys = Polygons()
o = checkHuman(obs_fil, prevHuman=self.prevHuman)
self.prevHuman = o
cen = o.center_sq()
human_cen = Point32()
human_cen.x = cen[0]
human_cen.y = cen[1]
for obj in obs_fil:
polys.polygons.append(obj.poly)
polys.vels.append(obj.toVel())
if self.mode == 0:
human_pub.publish(human_cen)
human_obs_pub.publish(polys)
human_v_pub.publish(o.toVel())
elif self.mode == 1:
goal = Point32()
goal.x = human_cen.x
goal.y = human_cen.y - 1.5
goal_pub.publish(goal)
human_pub.publish(human_cen)
human_obs_pub.publish(polys)
human_v_pub.publish(o.toVel())
rospy.Subscriber('/publish_line_markers', Marker, find_lines_and_pub)
#rospy.Subscriber('/scan', LaserScan, find_lines_and_pub)
#rospy.Subscriber('/scan_filtered', LaserScan, find_lines_and_pub)
rospy.spin()
class AccurateDocking(RComponent):
"""
A class used to make a simple docking process
"""
def __init__(self):
RComponent.__init__(self)
self.step = -1
# array of {'initial': [x,y,theta], 'final': [x,y,theta]}
self.results = []
self.ongoing_result = {}
self.total_iterations = 0
self.stop = False
def ros_read_params(self):
"""Gets params from param server"""
RComponent.ros_read_params(self)
self.dock_action_name = rospy.get_param('~dock_action_server',
default='pp_docker')
self.move_action_name = rospy.get_param('~move_action_server',
default='move')
self.robot_link = rospy.get_param('~base_frame',
default='robot_base_link')
self.pregoal_link = rospy.get_param(
'~pregoal_frame', default='laser_test_pregoal_footprint')
self.goal_link = rospy.get_param('~goal_frame',
default='laser_test_goal_footprint')
self.reflectors_link = rospy.get_param(
'~reflectors_frame',
default='robot_filtered_docking_station_laser')
# array of [x, y, theta] for the first dock call
param_offset_1 = rospy.get_param('~pregoal_offset_1',
default='[-0.5, 0, 0]')
self.pregoal_offset_1 = (param_offset_1.replace('[', '').replace(
']', '').replace(',', '')).split(' ')
if len(self.pregoal_offset_1) != 3:
self.pregoal_offset_1 = [0, 0, 0]
else:
for i in range(len(self.pregoal_offset_1)):
self.pregoal_offset_1[i] = float(self.pregoal_offset_1[i])
# array of [x, y, theta] for the second dock call
param_offset_2 = rospy.get_param('~pregoal_offset_2',
default='[-0.1, 0, 0]')
self.pregoal_offset_2 = (param_offset_2.replace('[', '').replace(
']', '').replace(',', '')).split(' ')
if len(self.pregoal_offset_2) != 3:
self.pregoal_offset_2 = [0, 0, 0]
else:
for i in range(len(self.pregoal_offset_2)):
self.pregoal_offset_2[i] = float(self.pregoal_offset_2[i])
rospy.loginfo("%s::ros_read_params: docker offsets: %s - %s",
rospy.get_name(), str(self.pregoal_offset_1),
str(self.pregoal_offset_2))
self.step_back_distance = rospy.get_param('~step_back_distance',
default=1.3)
self.consecutive_iterations = rospy.get_param(
'~consecutive_iterations', default=1)
self.current_iteration = self.consecutive_iterations
def ros_setup(self):
"""Creates and inits ROS components"""
RComponent.ros_setup(self)
self.tf = TransformListener()
self.move_action_client = actionlib.SimpleActionClient(
self.move_action_name, MoveAction)
self.dock_action_client = actionlib.SimpleActionClient(
self.dock_action_name, DockAction)
self.start_docking_server = rospy.Service(
'~start', Trigger, self.start_docking_service_cb)
self.stop_docking_server = rospy.Service('~stop', Trigger,
self.stop_docking_service_cb)
self.save_results_server = rospy.Service('~save_results', Empty,
self.save_results_service_cb)
self.set_pregoal_offset_1 = rospy.Service('~set_pregoal_offset_1',
SetTransform,
self.set_pregoal_offset_1_cb)
self.set_pregoal_offset_2 = rospy.Service('~set_pregoal_offset_2',
SetTransform,
self.set_pregoal_offset_2_cb)
self.docking_status_pub = rospy.Publisher('~status',
String,
queue_size=10)
self.docking_status = "stopped"
RComponent.ros_setup(self)
def ros_publish(self):
'''
Publish topics at standard frequency
'''
self.docking_status_pub.publish(self.docking_status)
RComponent.ros_publish(self)
def ready_state(self):
"""Actions performed in ready state"""
if self.current_iteration < self.consecutive_iterations:
self.docking_status = "running"
if self.step != -1:
try:
# Get relative goal position
t = self.tf.getLatestCommonTime(self.robot_link,
self.goal_link)
(position, quaternion) = self.tf.lookupTransform(
self.robot_link, self.goal_link, t)
(_, _, orientation) = euler_from_quaternion(quaternion)
except Exception as e:
rospy.logerr("%s::ready_state: exception: %s",
rospy.get_name(), e)
self.docking_status = "error"
self.stop = True
return
try:
# Get relative goal to the reflectors position
t = self.tf.getLatestCommonTime(self.robot_link,
self.reflectors_link)
(position_to_reflectors,
quaternion_to_reflectors) = self.tf.lookupTransform(
self.robot_link, self.reflectors_link, t)
(_, _, orientation_to_reflectors
) = euler_from_quaternion(quaternion_to_reflectors)
except Exception as e:
rospy.logerr("%s::ready_state: exception: %s",
rospy.get_name(), e)
self.docking_status = "error"
self.error_on_action()
return
if self.step == 0:
self.ongoing_result = {}
rospy.loginfo('%s::ros_setup: waiting for server %s',
rospy.get_name(), self.move_action_name)
self.move_action_client.wait_for_server()
rospy.loginfo('%s::ros_setup: waiting for server %s',
rospy.get_name(), self.dock_action_name)
self.dock_action_client.wait_for_server()
rospy.loginfo(
'%s::ready_state: Initial distance to goal-> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position[0] * 1000, position[1] * 1000,
math.degrees(orientation))
self.ongoing_result['initial'] = [
position[0], position[1],
math.degrees(orientation)
]
# Docking
dock_goal = DockGoal()
dock_goal.dock_frame = self.pregoal_link
dock_goal.robot_dock_frame = self.robot_link
dock_goal.dock_offset.x = self.pregoal_offset_1[0]
dock_goal.dock_offset.y = self.pregoal_offset_1[1]
dock_goal.dock_offset.theta = self.pregoal_offset_1[2]
rospy.loginfo(
'%s::ready_state: %d - docking to %s - offset = %s',
rospy.get_name(), self.step, self.pregoal_link,
str(dock_goal.dock_offset))
self.dock_action_client.send_goal(dock_goal)
self.dock_action_client.wait_for_result()
result = self.dock_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
self.step = 1
rospy.sleep(2)
else:
rospy.logerr("%s::ready_state: Docking failed",
rospy.get_name())
self.docking_status = "error"
self.error_on_action()
elif self.step == 1:
# Docking
dock_goal = DockGoal()
dock_goal.dock_frame = self.pregoal_link
dock_goal.robot_dock_frame = self.robot_link
dock_goal.dock_offset.x = self.pregoal_offset_2[0]
dock_goal.dock_offset.y = self.pregoal_offset_2[1]
dock_goal.dock_offset.theta = self.pregoal_offset_2[2]
rospy.loginfo(
'%s::ready_state: %d - docking to %s - offset = %s',
rospy.get_name(), self.step, self.pregoal_link,
str(dock_goal.dock_offset))
self.dock_action_client.send_goal(dock_goal)
self.dock_action_client.wait_for_result()
result = self.dock_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
self.step = 2
else:
rospy.logerr("%s::ready_state: Docking failed",
rospy.get_name())
self.error_on_action()
elif self.step == 2:
# Orientate robot
move_goal = MoveGoal()
move_goal.goal.theta = orientation
if abs(math.degrees(orientation)) > 0.2:
rospy.loginfo(
'%s::ready_state: %d - rotating %.3lf degrees to %s',
rospy.get_name(), self.step, math.degrees(orientation),
self.goal_link)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step,
str(result.success))
if result.success == True:
self.step = 3
else:
rospy.logerr("%s::ready_state: Move-Rotation failed",
rospy.get_name())
self.error_on_action()
self.docking_status = "error"
else:
rospy.loginfo(
'%s::ready_state: %d - avoids rotating %.3lf degrees to %s',
rospy.get_name(), self.step, math.degrees(orientation),
self.goal_link)
self.step = 3
elif self.step == 3:
# Move forward
move_goal = MoveGoal()
move_goal.goal.x = position[0]
rospy.loginfo(
'%s::ready_state: %d - moving forward %.3lf meters to %s',
rospy.get_name(), self.step, move_goal.goal.x,
self.goal_link)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
rospy.sleep(2)
self.step = 4
else:
rospy.logerr("%s::ready_state: Move-Forward failed",
rospy.get_name())
self.error_on_action()
self.docking_status = "error"
elif self.step == 4:
rospy.loginfo(
'%s::ready_state: final distance to goal -> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position[0] * 1000, position[1] * 1000,
math.degrees(orientation))
rospy.loginfo(
'%s::ready_state: final distance to reflectors -> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position_to_reflectors[0] * 1000,
position_to_reflectors[1] * 1000,
math.degrees(orientation_to_reflectors))
self.ongoing_result['final'] = [
position[0], position[1],
math.degrees(orientation)
]
self.results.append(self.ongoing_result)
if self.step_back_distance == 0.0:
self.iteration_success()
else:
self.step = 5
elif self.step == 5:
# Move backward
move_goal = MoveGoal()
move_goal.goal.x = -self.step_back_distance
rospy.loginfo(
'%s::ready_state: %d - moving backwards %.3lf meters',
rospy.get_name(), self.step, move_goal.goal.x)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
rospy.sleep(2)
self.iteration_success()
else:
rospy.logerr("%s::ready_state: Move-Forward failed",
rospy.get_name())
self.error_on_action()
self.docking_status = "error"
else:
self.docking_status = "stopped"
def error_on_action(self):
self.step = -1
self.current_iteration = self.consecutive_iterations
def iteration_success(self):
self.current_iteration += 1
self.total_iterations += 1
rospy.loginfo('%s::iteration_success: current it = %d - total = %s',
rospy.get_name(), self.current_iteration,
self.total_iterations)
if self.stop == True:
self.stop = False
if self.current_iteration < self.consecutive_iterations:
rospy.logwarn(
'%s::iteration_success: stopping due to user requirement',
rospy.get_name())
self.current_iteration = self.consecutive_iterations
if self.current_iteration < self.consecutive_iterations:
self.step = 0
else:
self.step = -1
def shutdown(self):
return RComponent.shutdown(self)
def switch_to_state(self, new_state):
"""Performs the change of state"""
return RComponent.switch_to_state(self, new_state)
def stop_docking_service_cb(self, req):
'''
stop docking process
'''
if self.current_iteration < self.consecutive_iterations:
self.stop = True
return True, "Stopping after this iteration (%d of %d) Step = %d" % (
self.current_iteration + 1, self.consecutive_iterations,
self.step)
else:
return False, "Docking not running"
def start_docking_service_cb(self, req):
'''
start docking process
'''
if self.current_iteration < self.consecutive_iterations:
return False, "Docking is still running. Iteration = (%d of %d) Step = %d" % (
self.current_iteration + 1, self.consecutive_iterations,
self.step)
else:
self.current_iteration = 0
self.step = 0
self.stop = False
return True, "Starting"
def save_results_service_cb(self, req):
'''
saves current results process
'''
rp = rospkg.RosPack()
filename = 'docking_%s.csv' % (str(datetime.datetime.now()).replace(
' ', '_').replace('.', '').replace(':', '-'))
folder = os.path.join(rp.get_path('accurate_docking'), 'data')
file_path = folder + '/' + filename
with open(file_path, 'w') as f:
for result in self.results:
f.write("%f,%f,%f,%f,%f,%f\n" %
(result['initial'][0], result['initial'][1],
result['initial'][2], result['final'][0],
result['final'][1], result['final'][2]))
print(self.results)
return []
def set_pregoal_offset_1_cb(self, req):
'''
modify pregoal_offset_1
'''
self.pregoal_offset_1[0] = req.tf.translation.x
self.pregoal_offset_1[1] = req.tf.translation.y
self.pregoal_offset_1[2] = req.tf.translation.z
ret = ReturnMessage()
ret.success = True
ret.message = "Changed pregoal_offset_1"
return ret
def set_pregoal_offset_2_cb(self, req):
'''
modify pregoal_offset_2
'''
self.pregoal_offset_2[0] = req.tf.translation.x
self.pregoal_offset_2[1] = req.tf.translation.y
self.pregoal_offset_2[2] = req.tf.translation.z
ret = ReturnMessage()
ret.success = True
ret.message = "Changed pregoal_offset_2"
return ret
class GripperActionController():
def __init__(self, controller_namespace, controllers):
self.controller_namespace = controller_namespace
self.controllers = {}
for c in controllers:
self.controllers[c.controller_namespace] = c
def initialize(self):
l_controller_name = rospy.get_param('~left_controller_name', 'left_finger_controller')
r_controller_name = rospy.get_param('~right_controller_name', 'right_finger_controller')
if l_controller_name not in self.controllers:
rospy.logerr('left_controller_name not found, requested: [%s], available: %s' % (l_controller_name, self.controllers))
return False
if r_controller_name not in self.controllers:
rospy.logerr('right_controller_name not found, requested: [%s], available: %s' % (r_controller_name, self.controllers))
return False
self.l_finger_controller = self.controllers[l_controller_name]
self.r_finger_controller = self.controllers[r_controller_name]
if self.l_finger_controller.port_namespace != self.r_finger_controller.port_namespace:
rospy.logerr('%s and %s are connected to different serial ports, this is unsupported' % (l_controller_name, r_controller_name))
return False
# left and right finger are assumed to be connected to the same port
self.dxl_io = self.l_finger_controller.dxl_io
self.l_finger_state = self.l_finger_controller.joint_state
self.r_finger_state = self.r_finger_controller.joint_state
self.l_max_speed = self.l_finger_controller.joint_max_speed
self.r_max_speed = self.r_finger_controller.joint_max_speed
return True
def start(self):
self.tf_listener = TransformListener()
# Publishers
self.gripper_opening_pub = rospy.Publisher('gripper_opening', Float64)
self.l_finger_ground_distance_pub = rospy.Publisher('left_finger_ground_distance', Float64)
self.r_finger_ground_distance_pub = rospy.Publisher('right_finger_ground_distance', Float64)
# Pressure sensors
# 0-3 - left finger
# 4-7 - right finger
num_sensors = 8
self.pressure = [0.0] * num_sensors
[rospy.Subscriber('/interface_kit/124427/sensor/%d' % i, Float64Stamped, self.process_pressure_sensors, i) for i in range(num_sensors)]
# pressure sensors are at these values when no external pressure is applied
self.l_zero_pressure = [0.0, 0.0, 0.0, 0.0]
self.r_zero_pressure = [0.0, 0.0, 130.0, 0.0]
self.lr_zero_pressure = self.l_zero_pressure + self.r_zero_pressure
self.l_total_pressure_pub = rospy.Publisher('left_finger_pressure', Float64)
self.r_total_pressure_pub = rospy.Publisher('right_finger_pressure', Float64)
self.lr_total_pressure_pub = rospy.Publisher('total_pressure', Float64)
self.close_gripper = False
self.dynamic_torque_control = False
self.lower_pressure = 800.0
self.upper_pressure = 1000.0
# IR sensor
self.gripper_ir_pub = rospy.Publisher('gripper_distance_sensor', Float64)
self.ir_distance = 0.0
rospy.Subscriber('/interface_kit/106950/sensor/7', Float64Stamped, self.process_ir_sensor)
# Temperature monitor and torque control thread
Thread(target=self.gripper_monitor).start()
# Start gripper opening monitoring thread
Thread(target=self.calculate_gripper_opening).start()
self.action_server = actionlib.SimpleActionServer('wubble_gripper_action',
WubbleGripperAction,
execute_cb=self.process_gripper_action,
auto_start=False)
self.action_server.start()
rospy.loginfo('gripper_freespin_controller: ready to accept goals')
def stop(self):
pass
def __send_motor_command(self, l_desired_torque, r_desired_torque):
l_motor_id = self.l_finger_controller.motor_id
r_motor_id = self.r_finger_controller.motor_id
l_trq = self.l_finger_controller.spd_rad_to_raw(l_desired_torque)
r_trq = self.r_finger_controller.spd_rad_to_raw(r_desired_torque)
self.dxl_io.set_multi_speed( ( (l_motor_id,l_trq), (r_motor_id,r_trq) ) )
def activate_gripper(self, command, torque_limit):
if command == WubbleGripperGoal.CLOSE_GRIPPER:
l_desired_torque = -torque_limit * self.l_max_speed
r_desired_torque = torque_limit * self.r_max_speed
else: # assume opening
l_desired_torque = torque_limit * self.l_max_speed
r_desired_torque = -torque_limit * self.r_max_speed
self.__send_motor_command(l_desired_torque, r_desired_torque)
return max(l_desired_torque, r_desired_torque)
def gripper_monitor(self):
rospy.loginfo('Gripper temperature monitor and torque control thread started successfully')
motors_overheating = False
max_pressure = 8000.0
r = rospy.Rate(150)
while not rospy.is_shutdown():
l_total_pressure = max(0.0, sum(self.pressure[:4]) - sum(self.l_zero_pressure))
r_total_pressure = max(0.0, sum(self.pressure[4:]) - sum(self.r_zero_pressure))
pressure = l_total_pressure + r_total_pressure
self.l_total_pressure_pub.publish(l_total_pressure)
self.r_total_pressure_pub.publish(r_total_pressure)
self.lr_total_pressure_pub.publish(pressure)
#----------------------- TEMPERATURE MONITOR ---------------------------#
l_temp = max(self.l_finger_state.motor_temps)
r_temp = max(self.r_finger_state.motor_temps)
if l_temp >= 75 or r_temp >= 75:
if not motors_overheating:
rospy.logwarn('Disabling gripper motors torque [LM: %dC, RM: %dC]' % (l_temp, r_temp))
self.__send_motor_command(-0.5, 0.5)
motors_overheating = True
else:
motors_overheating = False
#########################################################################
# don't do torque control if not requested or
# when the gripper is open
# or when motors are too hot
if not self.dynamic_torque_control or \
not self.close_gripper or \
motors_overheating:
r.sleep()
continue
#----------------------- TORQUE CONTROL -------------------------------#
l_current = self.l_finger_state.goal_pos
r_current = self.r_finger_state.goal_pos
if pressure > self.upper_pressure: # release
pressure_change_step = abs(pressure - self.upper_pressure) / max_pressure
l_goal = min( self.l_max_speed, l_current + pressure_change_step)
r_goal = max(-self.r_max_speed, r_current - pressure_change_step)
if l_goal < self.l_max_speed or r_goal > -self.r_max_speed:
if self.close_gripper:
self.__send_motor_command(l_goal, r_goal)
rospy.logdebug('>MAX pressure is %.2f, LT: %.2f, RT: %.2f, step is %.2f' % (pressure, l_current, r_current, pressure_change_step))
elif pressure < self.lower_pressure: # squeeze
pressure_change_step = abs(pressure - self.lower_pressure) / max_pressure
l_goal = max(-self.l_max_speed, l_current - pressure_change_step)
r_goal = min( self.r_max_speed, r_current + pressure_change_step)
if l_goal > -self.l_max_speed or r_goal < self.r_max_speed:
if self.close_gripper:
self.__send_motor_command(l_goal, r_goal)
rospy.logdebug('<MIN pressure is %.2f, LT: %.2f, RT: %.2f, step is %.2f' % (pressure, l_current, r_current, pressure_change_step))
########################################################################
r.sleep()
def calculate_gripper_opening(self):
timeout = rospy.Duration(5)
last_reported = rospy.Time(0)
r = rospy.Rate(50)
while not rospy.is_shutdown():
try:
map_frame_id = 'base_footprint'
palm_frame_id = 'L7_wrist_roll_link'
l_fingertip_frame_id = 'left_fingertip_link'
r_fingertip_frame_id = 'right_fingertip_link'
self.tf_listener.waitForTransform(palm_frame_id, l_fingertip_frame_id, rospy.Time(0), rospy.Duration(0.2))
self.tf_listener.waitForTransform(palm_frame_id, r_fingertip_frame_id, rospy.Time(0), rospy.Duration(0.2))
l_pos, _ = self.tf_listener.lookupTransform(palm_frame_id, l_fingertip_frame_id, rospy.Time(0))
r_pos, _ = self.tf_listener.lookupTransform(palm_frame_id, r_fingertip_frame_id, rospy.Time(0))
dx = l_pos[0] - r_pos[0]
dy = l_pos[1] - r_pos[1]
dz = l_pos[2] - r_pos[2]
dist = math.sqrt(dx*dx + dy*dy + dz*dz)
self.gripper_opening_pub.publish(dist)
l_pos, _ = self.tf_listener.lookupTransform(map_frame_id, l_fingertip_frame_id, rospy.Time(0))
r_pos, _ = self.tf_listener.lookupTransform(map_frame_id, r_fingertip_frame_id, rospy.Time(0))
self.l_finger_ground_distance_pub.publish(l_pos[2])
self.r_finger_ground_distance_pub.publish(r_pos[2])
except LookupException as le:
rospy.logerr(le)
except ConnectivityException as ce:
rospy.logerr(ce)
except tf.Exception as e:
if rospy.Time.now() - last_reported > timeout:
rospy.logerr('TF exception: %s' % str(e))
last_reported = rospy.Time.now()
r.sleep()
def process_pressure_sensors(self, msg, i):
self.pressure[i] = msg.data
def process_ir_sensor(self, msg):
"""
Given a raw sensor value from Sharp IR sensor (4-30cm model)
returns an actual distance in meters.
"""
if msg.data < 80: self.ir_distance = 1.0 # too far
elif msg.data > 530: self.ir_distance = 0.0 # too close
else: self.ir_distance = 20.76 / (msg.data - 11) # just right
self.gripper_ir_pub.publish(self.ir_distance)
def process_gripper_action(self, req):
r = rospy.Rate(500)
timeout = rospy.Duration(2.0)
if req.command == WubbleGripperGoal.CLOSE_GRIPPER:
self.dynamic_torque_control = req.dynamic_torque_control
self.lower_pressure = req.pressure_lower
self.upper_pressure = req.pressure_upper
desired_torque = self.activate_gripper(req.command, req.torque_limit)
if not self.dynamic_torque_control:
start_time = rospy.Time.now()
while not rospy.is_shutdown() and \
rospy.Time.now() - start_time < timeout and \
(not within_tolerance(self.l_finger_state.load, -req.torque_limit, 0.01) or
not within_tolerance(self.r_finger_state.load, -req.torque_limit, 0.01)):
r.sleep()
else:
if desired_torque > 1e-3: rospy.sleep(1.0 / desired_torque)
self.close_gripper = True
self.action_server.set_succeeded()
elif req.command == WubbleGripperGoal.OPEN_GRIPPER:
self.close_gripper = False
self.activate_gripper(req.command, req.torque_limit)
start_time = rospy.Time.now()
while not rospy.is_shutdown() and \
rospy.Time.now() - start_time < timeout and \
(self.l_finger_state.current_pos < 0.8 or
self.r_finger_state.current_pos > -0.8):
r.sleep()
self.__send_motor_command(0.5, -0.5)
self.action_server.set_succeeded()
else:
msg = 'Unrecognized command: %d' % req.command
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except:
rospy.logwarn('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
#TODO: Send the arms to ik_solution
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform, offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
class Tensioner(object):
def __init__(self):
self.torque = Gripper_Torque()
self.tl = TransformListener()
def get_translation(self,direction):
pose = self.tl.lookupTransform(direction,'hand_palm_link',rospy.Time(0))
trans = pose[0]
return trans
def get_rotation(self,direction):
pose = self.tl.lookupTransform(direction,'hand_palm_link',rospy.Time(0))
rot = pose[1]
return tf.transformations.quaternion_matrix(rot)
def compute_bed_tension(self, wrench, direction):
x = wrench.wrench.force.x
y = wrench.wrench.force.y
z = wrench.wrench.force.z
force = np.array([x,y,z,1.0])
rot = self.get_rotation(direction)
force_perp = np.dot(rot,force)
print("\nInside p_pi/bed_making/tensioner.py, `compute_bed_tension()`:")
print("CURRENT FORCES: {}".format(force))
print("FORCE PERP: {}".format(force_perp))
return force_perp[1]
def force_pull(self, whole_body, direction):
"""Pull to the target `direction`, hopefully with the sheet!
I think this splits it into several motions, at most `cfg.MAX_PULLS`, so
this explains the occasional pauses with the HSR's motion. This is *per*
grasp+pull attempt, so it's different from the max attempts per side,
which I empirically set at 4. Actually he set his max pulls to 3, but
because of the delta <= 1.0 (including equality) this is 4 also.
Move the end-effector, then quickly read the torque data and look at the
wrench. The end-effector moves according to a _fraction_ that is
specified by `s = 1-delta`, so I _hope_ if there's too much tension
after that tiny bit of motion, that we'll exit. But maybe the
discretization into four smaller pulls is not fine enough?
"""
count = 0
is_pulling = False
t_o = self.get_translation(direction)
delta = 0.0
while delta <= 1.0:
s = 1.0 - delta
whole_body.move_end_effector_pose(
geometry.pose(x=t_o[0]*s, y=t_o[1]*s, z=t_o[2]*s),
direction
)
wrench = self.torque.read_data()
norm = np.abs(self.compute_bed_tension(wrench,direction))
print("FORCE NORM: {}".format(norm))
if norm > cfg.HIGH_FORCE:
is_pulling = True
if is_pulling and norm < cfg.LOW_FORCE:
break
if norm > cfg.FORCE_LIMT:
break
delta += 1.0/float(cfg.MAX_PULLS)
class Head:
# 'LOOK_FORWARD: 'LOOK_FORWARD',
# 'FOLLOW_RIGHT_EE: 'FOLLOW_RIGHT_EE',
# 'FOLLOW_LEFT_EE: 'FOLLOW_LEFT_EE',
# 'GLANCE_RIGHT_EE: 'GLANCE_RIGHT_EE',
# 'GLANCE_LEFT_EE: 'GLANCE_LEFT_EE',
# 'NOD: 'NOD',
# 'SHAKE: 'SHAKE',
# 'FOLLOW_FACE: 'FOLLOW_FACE',
# 'LOOK_AT_POINT: 'LOOK_AT_POINT',
# 'LOOK_DOWN: 'LOOK_DOWN',
# 'NOD_ONCE : 'NOD_ONCE ',
# 'SHAKE_ONCE: 'SHAKE_ONCE',
def __init__(self):
self.defaultLookatPoint = Point(1,0,1.35)
self.downLookatPoint = Point(0.5,0,0.5)
self.targetLookatPoint = Point(1,0,1.35)
self.currentLookatPoint = Point(1,0,1.35)
self.currentGazeAction = 'LOOK_FORWARD';
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint);
# Some constants
self.doNotInterrupt = ['GLANCE_RIGHT_EE', 'GLANCE_LEFT_EE', 'NOD', 'SHAKE'];
self.nodPositions = [Point(1,0,1.05), Point(1,0,1.55)]
self.shakePositions = [Point(1,0.2,1.35), Point(1,-0.2,1.35)]
self.nNods = 5
self.nShakes = 5
self.nodCounter = 5
self.shakeCounter = 5
self.facePos = None
## Action client for sending commands to the head.
self.headActionClient = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self.headActionClient.wait_for_server()
rospy.loginfo('Head action client has responded.')
self.headGoal = PointHeadGoal()
self.headGoal.target.header.frame_id = 'base_link'
self.headGoal.min_duration = rospy.Duration(1.0)
self.headGoal.target.point = Point(1,0,1)
## Client for receiving detected faces
#self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction)
## Service client for arm states
self.tfListener = TransformListener()
## Callback function to receive ee states and face location
def getEEPos(self, armIndex):
fromFrame = '/base_link'
if (armIndex == 0):
toFrame = '/r_wrist_roll_link'
else:
toFrame = '/l_wrist_roll_link'
try:
t = self.tfListener.getLatestCommonTime(fromFrame, toFrame)
(position, rotation) = self.tfListener.lookupTransform(fromFrame, toFrame, t)
except:
rospy.logerr('Could not get the end-effector pose.')
#objPoseStamped = PoseStamped()
#objPoseStamped.header.stamp = t
#objPoseStamped.header.frame_id = '/base_link'
#objPoseStamped.pose = Pose()
#relEEPose = self.tfListener.transformPose(toFrame, objPoseStamped)
return Point(position[0], position[1], position[2])
def getFaceLocation(self):
connected = self.faceClient.wait_for_server(rospy.Duration(1.0))
if connected:
fgoal = FaceDetectorGoal()
self.faceClient.send_goal(fgoal)
self.faceClient.wait_for_result()
f = self.faceClient.get_result()
## If there is one face follow that one, if there are more than one, follow the closest one
closest = -1
if len(f.face_positions) == 1:
closest = 0
elif len(f.face_positions) > 0:
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y + f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
self.facePos = array([f.face_positions[closest].pos.x, f.face_positions[closest].pos.y, f.face_positions[closest].pos.z])
else:
rospy.logwarn('No faces were detected.')
self.facePos = self.defaultLookatPoint
else:
rospy.logwarn('Not connected to the face server, cannot follow faces.')
self.facePos = self.defaultLookatPoint
## Callback function for receiving gaze commands
def do_gaze_action(self, command):
#command = goal.action;
if (self.doNotInterrupt.count(self.currentGazeAction) == 0):
if (self.currentGazeAction != command or command == 'LOOK_AT_POINT'):
self.isActionComplete = False
if (command == 'LOOK_FORWARD'):
self.targetLookatPoint = self.defaultLookatPoint
elif (command == 'LOOK_DOWN'):
self.targetLookatPoint = self.downLookatPoint
elif (command == 'NOD'):
self.nNods = 5
self.startNod()
elif (command == 'SHAKE'):
self.nShakes = 5
self.startShake()
elif (command == 'NOD_ONCE'):
self.nNods = 5
self.startNod()
elif (command == 'SHAKE_ONCE'):
self.nShakes = 5
self.startShake()
elif (command == 'GLANCE_RIGHT_EE'):
self.startGlance(0)
elif (command == 'GLANCE_LEFT_EE'):
self.startGlance(1)
elif (command == 'LOOK_AT_POINT'):
self.targetLookatPoint = goal.point
rospy.loginfo('\tSetting gaze action to: ' +
command)
self.currentGazeAction = command
while (not self.isActionComplete):
time.sleep(0.1)
def isTheSame(self, current, target):
diff = target - current
dist = linalg.norm(diff)
return (dist<0.0001)
def filterLookatPosition(self, current, target):
speed = 0.02
diff = self.point2array(target) - self.point2array(current)
dist = linalg.norm(diff)
if (dist>speed):
step = dist/speed
return self.array2point(self.point2array(current) + diff/step)
else:
return target
def startNod(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.nodCounter = 0
self.targetLookatPoint = self.nodPositions[0]
def startGlance(self, armIndex):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.glanceCounter = 0
self.targetLookatPoint = self.getEEPos(armIndex)
def startShake(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.shakeCounter = 0
self.targetLookatPoint = self.shakePositions[0]
def point2array(self, p):
return array((p.x, p.y, p.z))
def array2point(self, a):
return Point(a[0], a[1], a[2])
def getNextNodPoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.nodCounter += 1
if (self.nodCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.nodPositions[self.nodCounter%2]
else:
return target
def getNextGlancePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.glanceCounter = 1
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return target
def getNextShakePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.shakeCounter += 1
if (self.shakeCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.shakePositions[self.shakeCounter%2]
else:
return target
def update(self):
isActionPossiblyComplete = True
if (self.currentGazeAction == 'FOLLOW_RIGHT_EE'):
self.targetLookatPoint = self.getEEPos(0)
elif (self.currentGazeAction == 'FOLLOW_LEFT_EE'):
self.targetLookatPoint = self.getEEPos(1)
elif (self.currentGazeAction == 'FOLLOW_FACE'):
self.getFaceLocation()
self.targetLookatPoint = self.facePos
elif (self.currentGazeAction == 'NOD'):
self.targetLookatPoint = self.getNextNodPoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == 'SHAKE'):
self.targetLookatPoint = self.getNextShakePoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == 'GLANCE_RIGHT_EE' or self.currentGazeAction == 'GLANCE_LEFT_EE'):
self.targetLookatPoint = self.getNextGlancePoint(self.currentLookatPoint, self.targetLookatPoint)
isActionPossiblyComplete = False;
self.currentLookatPoint = self.filterLookatPosition(self.currentLookatPoint, self.targetLookatPoint)
if (self.isTheSame(self.point2array(self.headGoal.target.point), self.point2array(self.currentLookatPoint))):
if (isActionPossiblyComplete):
if (self.headActionClient.get_state() == GoalStatus.SUCCEEDED):
self.isActionComplete = True
else:
self.headGoal.target.point = self.currentLookatPoint
self.headActionClient.send_goal(self.headGoal)
time.sleep(0.02)
class CardPicker():
def __init__(self):
'''
Initialization class for a Policy
Parameters
----------
yumi : An instianted yumi robot
com : The common class for the robot
cam : An open bincam class
debug : bool
A bool to indicate whether or not to display a training set point for
debuging.
'''
self.robot = hsrb_interface.Robot()
self.omni_base = self.robot.get('omni_base')
self.whole_body = self.robot.get('whole_body')
self.cam = RGBD()
self.com = COM()
self.com.go_to_initial_state(self.whole_body)
self.br = tf.TransformBroadcaster()
self.tl = TransformListener()
self.gp = GraspPlanner()
self.suction = Suction(self.gp, self.cam)
#thread.start_new_thread(self.ql.run,())
print "after thread"
def find_mean_depth(self, d_img):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
indx = np.nonzero(d_img)
mean = np.mean(d_img[indx])
return
def card_pick(self):
while True:
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
if (not c_img == None and not d_img == None):
self.ql = QueryLabeler()
self.ql.run(c_img)
data = self.ql.label_data
del self.ql
self.suction.find_pick_region(data, c_img, d_img)
# card_found,cards = self.check_card_found()
# if(card_found):
# self.suction.execute_grasp(cards,self.whole_body)
# self.com.go_to_initial_state(self.whole_body)
def check_card_found(self):
# try:
transforms = self.tl.getFrameStrings()
cards = []
for transform in transforms:
print transform
if 'card' in transform:
print 'got here'
f_p = self.tl.lookupTransform('head_rgbd_sensor_rgb_frame',
transform, rospy.Time(0))
cards.append(transform)
return True, cards
class HandEyeCalibrator(object):
def __init__(self):
self._camera_frame = rospy.get_param('%s/camera_frame' % (NODE_NAME, ),
'camera_rgb_optical_frame')
self._marker_frame = rospy.get_param('%s/marker_frame' % (NODE_NAME, ),
'marker26')
self._robot_base_frame = rospy.get_param(
'%s/robot_base_frame' % (NODE_NAME, ), 'world')
self._ee_frame = rospy.get_param('%s/ee_frame' % (NODE_NAME, ),
'left_arm_7_link')
self._save_calib_file = rospy.get_param(
'%s/save_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy')
self._load_calib_file = rospy.get_param(
'%s/load_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy')
self._calib_from_file = rospy.get_param(
'%s/calibrate_from_file' % (NODE_NAME, ), False)
rospy.loginfo("Camera frame: %s" % (self._camera_frame, ))
rospy.loginfo("Marker frame: %s" % (self._marker_frame, ))
rospy.loginfo("Robot frame: %s" % (self._robot_base_frame, ))
rospy.loginfo("End-effector frame: %s" % (self._ee_frame, ))
rospy.loginfo("Calibration filename to save: %s" %
(self._save_calib_file, ))
rospy.loginfo("Calibration filename to load: %s" %
(self._load_calib_file, ))
rospy.loginfo("Calibrate from file: %s" % (self._calib_from_file, ))
self._camera_base_p = [0.000, 0.022, 0.0]
self._camera_base_q = [-0.500, 0.500, -0.500, 0.500]
self._camera_base_transform = self.to_transform_matrix(
self._camera_base_p, self._camera_base_q)
self._calib = HandEyeCalib()
self._br = tf.TransformBroadcaster()
self._tf = TransformListener()
def broadcast_frame(self, pt, rot, frame_name="marker"):
rot = np.append(rot, [[0, 0, 0]], 0)
rot = np.append(rot, [[0], [0], [0], [1]], 1)
quat = tuple(tf.transformations.quaternion_from_matrix(rot))
now = rospy.Time.now()
self._br.sendTransform((pt[0], pt[1], pt[2]),
tf.transformations.quaternion_from_matrix(rot),
now, frame_name, 'base')
print("should have done it!")
def get_transform(self, base_frame, source_frame):
if base_frame == source_frame:
return (0, 0, 0), (0, 0, 0, 1), rospy.Time.now()
# t = (0,0,0)
# q = (0,0,0,1)
time = None
while time is None:
try:
time = self._tf.getLatestCommonTime(source_frame, base_frame)
except:
rospy.loginfo(
"Failed to get common time between %s and %s. Trying again..."
% (
source_frame,
base_frame,
))
t = None
q = None
try:
t, q = self._tf.lookupTransform(
base_frame, source_frame, time
) #self._tf_buffer.lookup_transform(frame_name, base_frame, rospy.Time(0), rospy.Duration(5.0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr("Transform could not be queried.")
return (0, 0, 0), (0, 0, 0, 1), rospy.Time.now()
translation = (t[0], t[1], t[2])
quaternion = (q[0], q[1], q[2], q[3])
# translation = (t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)
# quaternion = (t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w)
return translation, quaternion, time
def to_transform_matrix(self, t, q):
t_mat = tf.transformations.translation_matrix(t)
r_mat = tf.transformations.quaternion_matrix(q)
transform_mat = np.dot(t_mat, r_mat)
return transform_mat
def to_euler(self, transform):
return np.asarray(tf.transformations.euler_from_matrix(transform))
def to_translation(self, transform):
return np.array(transform[:3, 3])
def calibrate_from_file(self):
calib_data = np.load(self._load_calib_file).item()
camera_poses = calib_data['camera_poses']
ee_poses = calib_data['ee_poses']
for i in range(len(camera_poses)):
self._calib.add_measurement(ee_poses[i], camera_poses[i])
hand_eye_transform = self._calib.calibrate()
hand_eye_transform = np.matmul(
hand_eye_transform, np.linalg.inv(self._camera_base_transform))
print 'Hand-eye transform: ', hand_eye_transform
translation = self.to_translation(hand_eye_transform)
euler = self.to_euler(hand_eye_transform)
print "XYZ:", translation
print "RPY: ", euler
# calib_data = {'hand_eye_transform': {'matrix': hand_eye_transform,
# 'xyz': translation,
# 'rpy': euler},
# 'camera_poses': self._calib._camera_poses,
# 'ee_poses': self._calib._ee_poses}
# np.save('hand_eye_calibration_right.npy', calib_data)
def calibrate(self):
from getch import getch
calibrate = False
print(
"Press enter to record to next. Add/Remove/Calibrate/Help? (a/r/c/h)"
)
while not calibrate:
r = getch()
if r == 'a':
camera_t, camera_q, _ = self.get_transform(
self._camera_frame, self._marker_frame)
ee_t, ee_q, _ = self.get_transform(self._robot_base_frame,
self._ee_frame)
camera_transform = self.to_transform_matrix(camera_t, camera_q)
ee_transform = self.to_transform_matrix(ee_t, ee_q)
print "------------%d Poses---------------" % (len(
self._calib._ee_poses), )
print "Camera transform: ", camera_t, camera_q
print "Hand transform: ", ee_t, ee_q
print "---------------------------------"
self._calib.add_measurement(ee_transform, camera_transform)
print 'Transform has been added for calibration'
elif r == 'c':
calibrate = True
elif r == 'h':
print(
"Press enter to record to next. Add/Remove/Calibrate? (a/r/c/h)"
)
elif r == 'r':
if len(self._calib._ee_poses) > 0:
del self._calib._ee_poses[-1]
del self._calib._camera_poses[-1]
print("Removed last pose")
else:
print("Pose list already empty.")
elif r in ['\x1b', '\x03']:
rospy.signal_shutdown("Acquisition finished.")
break
if calibrate:
hand_eye_transform = self._calib.calibrate()
hand_eye_transform = np.matmul(
hand_eye_transform, np.linalg.inv(self._camera_base_transform))
print "Found Transform: ", hand_eye_transform
translation = self.to_translation(hand_eye_transform)
euler = self.to_euler(hand_eye_transform)
print "XYZ:", translation
print "RPY: ", euler
calib_data = {
'hand_eye_transform': {
'matrix': hand_eye_transform,
'xyz': translation,
'rpy': euler
},
'camera_poses': self._calib._camera_poses,
'ee_poses': self._calib._ee_poses
}
np.save('hand_eye_calibration_right.npy', calib_data)
np.save(self._save_calib_file, calib_data)
else:
print("Quiting. Bye!")
class mapping():
def __init__(self):
self.node_name = rospy.get_name()
self.tf = TransformListener()
self.transformer = TransformerROS()
rospy.loginfo("[%s] Initializing " % (self.node_name))
#rospy.Subscriber("/pcl_points", PoseArray, self.call_back, queue_size=1, buff_size = 2**24)
sub_pcl = message_filters.Subscriber("/pcl_points", PoseArray)
sub_odom = message_filters.Subscriber("/odometry/ground_truth",
Odometry)
ats = ApproximateTimeSynchronizer((sub_pcl, sub_odom),
queue_size=1,
slop=0.1)
ats.registerCallback(self.call_back)
rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.cb_new_goal,
queue_size=1)
self.pub_map = rospy.Publisher('/local_map',
OccupancyGrid,
queue_size=1)
self.pub_rviz = rospy.Publisher("/wp_line", Marker, queue_size=1)
self.pub_poses = rospy.Publisher("/path_points",
PoseArray,
queue_size=1)
self.resolution = 0.25
self.robot = Pose()
self.width = 200
self.height = 200
self.origin = Pose()
self.local_map = OccupancyGrid()
self.dilating_size = 6
self.wall_width = 3
self.start_planning = False
self.transpose_matrix = None
self.goal = []
self.astar = AStar()
self.msg_count = 0
self.planning_range = 20
self.frame_id = "map"
def init_param(self):
self.occupancygrid = np.zeros((self.height, self.width))
self.local_map = OccupancyGrid()
self.local_map.info.resolution = self.resolution
self.local_map.info.width = self.width
self.local_map.info.height = self.height
self.origin.position.x = -self.width * self.resolution / 2. + self.robot.position.x
self.origin.position.y = -self.height * self.resolution / 2. + self.robot.position.y
self.local_map.info.origin = self.origin
def cb_rviz(self, msg):
self.click_pt = [msg.pose.position.x, msg.pose.position.y]
self.publish_topic()
def call_back(self, pcl_msg, odom_msg):
self.robot = odom_msg.pose.pose
self.msg_count = self.msg_count + 1
self.init_param()
self.local_map.header = pcl_msg.header
self.local_map.header.frame_id = self.frame_id
self.get_tf()
if self.transpose_matrix is None:
return
for i in range(len(pcl_msg.poses)):
p = (pcl_msg.poses[i].position.x, pcl_msg.poses[i].position.y,
pcl_msg.poses[i].position.z, 1)
local_p = np.array(p)
global_p = np.dot(self.transpose_matrix, local_p)
x, y = self.map2occupancygrid(global_p)
width_in_range = (x >= self.width - self.dilating_size
or x <= self.dilating_size)
height_in_range = (y >= self.height - self.dilating_size
or y <= self.dilating_size)
if width_in_range or height_in_range:
continue # To prevent point cloud range over occupancy grid range
self.occupancygrid[y][x] = 100
# map dilating
for i in range(self.height):
for j in range(self.width):
if self.occupancygrid[i][j] == 100:
for m in range(-self.dilating_size,
self.dilating_size + 1):
for n in range(-self.dilating_size,
self.dilating_size + 1):
if self.occupancygrid[i + m][j + n] != 100:
if m > self.wall_width or m < -self.wall_width or n > self.wall_width or n < -self.wall_width:
if self.occupancygrid[i + m][j + n] != 90:
self.occupancygrid[i + m][j + n] = 50
else:
self.occupancygrid[i + m][j + n] = 90
for i in range(self.height):
for j in range(self.width):
self.local_map.data.append(self.occupancygrid[i][j])
self.pub_map.publish(self.local_map)
if self.start_planning:
self.path_planning()
def get_tf(self):
try:
position, quaternion = self.tf.lookupTransform(
"/map", "/X1/front_laser", rospy.Time(0))
self.transpose_matrix = self.transformer.fromTranslationRotation(
position, quaternion)
except (LookupException, ConnectivityException,
ExtrapolationException):
print("Nothing Happen")
def path_planning(self):
if self.msg_count % 5 != 0:
return
self.msg_count = 0
cost_map = np.zeros((self.height, self.width))
border = self.planning_range / self.resolution
h_min = int((self.height - border) / 2.)
h_max = int(self.height - (self.height - border) / 2.)
w_min = int((self.height - border) / 2.)
w_max = int(self.width - (self.width - border) / 2.)
for i in range(self.width):
for j in range(self.width):
if i > h_min and i < h_max:
if j > w_min and j < w_max:
cost_map[i][j] = self.occupancygrid[i][j]
start_point = self.map2occupancygrid(
(self.robot.position.x, self.robot.position.y))
start = (start_point[1], start_point[0])
goal = self.map2occupancygrid(self.goal)
end = (goal[1], goal[0])
self.astar.initial(cost_map, start, end)
path = self.astar.planning()
self.pub_path(path)
self.rviz(path)
def cb_new_goal(self, p):
self.goal = [p.pose.position.x, p.pose.position.y]
self.start_planning = True
def occupancygrid2map(self, p):
x = p[
0] * self.resolution + self.origin.position.x + self.resolution / 2.
y = p[
1] * self.resolution + self.origin.position.y + self.resolution / 2.
return [x, y]
def map2occupancygrid(self, p):
x = int((p[0] - self.origin.position.x) / self.resolution)
y = int((p[1] - self.origin.position.y) / self.resolution)
return [x, y]
def pub_path(self, path):
poses = PoseArray()
for i in range(len(path)):
p = self.occupancygrid2map([path[i][1], path[i][0]])
pose = Pose()
pose.position.x = p[0]
pose.position.y = p[1]
pose.position.z = 0
poses.poses.append(pose)
self.pub_poses.publish(poses)
def rviz(self, path):
marker = Marker()
marker.header.frame_id = self.frame_id
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.
marker.color.b = 0.
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.points = []
for i in range(len(path)):
p = self.occupancygrid2map([path[i][1], path[i][0]])
point = Point()
point.x = p[0]
point.y = p[1]
point.z = 0
marker.points.append(point)
self.pub_rviz.publish(marker)
class KinovaUtilities(object):
JOINT_1_MIN_MAX = (-3.14, 3.14)
JOINT_2_MIN_MAX = (0.82, 5.46)
JOINT_3_MIN_MAX = (0.33, 5.95)
JOINT_4_MIN_MAX = (-3.14, 3.14)
JOINT_5_MIN_MAX = (-3.14, 3.14)
JOINT_6_MIN_MAX = (-3.14, 3.14)
JOINT_4_OFFSET = 6.28325902769
JOINT_5_OFFSET = -18.8241170548
JOINT_6_OFFSET = 18.8293733406
CUBE_GZ_NAME = "cube"
X_CUBE_MIN = 0.76
X_CUBE_MAX = 1.03
Y_CUBE_MIN = 0.55
Y_CUBE_MAX = 1.05
def __init__(self):
super(KinovaUtilities, self).__init__()
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Kinova_Utilities', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
self.robot = moveit_commander.RobotCommander()
group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
self.scene = moveit_commander.PlanningSceneInterface()
self.box_name = "cube"
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
self.arm_group_name = "Arm"
self.arm_group = moveit_commander.MoveGroupCommander(self.arm_group_name)
print "============ Printing arm joint values"
#print self.arm_group.get_current_joint_values()
self.gripper_group_name = "All"
self.gripper_group = moveit_commander.MoveGroupCommander(self.gripper_group_name)
print "============ Printing gripper joint values"
#print self.gripper_group.get_current_joint_values()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print self.robot.get_current_state()
print ""
current_pose = self.arm_group.get_current_pose()
print "============ Current Pose"
print current_pose
print ""
# aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
# rospy.loginfo("Got: " + str(aruco_pose))
#self.return_home_position()
self.tf = TransformListener()
rate = rospy.Rate(10.0)
#print trans
# rand_x = random.uniform(self.X_CUBE_MIN, self.X_CUBE_MAX)
# rand_y = random.uniform(self.Y_CUBE_MIN, self.Y_CUBE_MAX)
#
# self.delete_gazebo_object(self.CUBE_GZ_NAME)
# self.spawn_gazebo_object(x=rand_x, y=rand_y, model_name=self.CUBE_GZ_NAME)
#
try:
self.tf.waitForTransform("/world", "/tag_0", rospy.Time(), rospy.Duration(4))
now = rospy.Time(0)
(trans, rot) = self.tf.lookupTransform("/world", "/tag_0", now)
self.add_rviz_object(self.CUBE_GZ_NAME, trans, rot)
except TransformException, e:
rospy.logerr("Could not find the TAG: {0}".format(e))
#self.add_object("cube", trans,rot)
# tag_pos = geometry_msgs.msg.PoseStamped()
# tag_pos.header.frame_id = "tag_0"
# tag_pos.pose.orientation.w = 1.0
# start_pos_manual = self.listener.transformPose("world", tag_pos)
# #
# rospy.loginfo("Pose via manual TF is: ")
# rospy.loginfo(start_pos_manual)
#
# self.return_home_position()
self.pre_grasp_face_4(trans)
# self.return_home_position()
# self.pre_grasp_face_2(trans)
# # #
# self.return_home_position()
# self.pre_grasp_face_1(trans)
# self.return_home_position()
try:
self.joint_planning()
except moveit_commander.MoveItCommanderException, e:
print(e)
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
self.pidX = PID(20, 12, 0.0, -30, 30, "x")
self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 0.5
self.lastJoy = Joy()
def _cmdVelTelopChanged(self, data):
self.cmd_vel_telop = data
if self.state == Controller.Manual:
self.pubNav.publish(data)
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _joyChanged(self, data):
if len(data.buttons) == len(self.lastJoy.buttons):
delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
print ("Buton ok")
#Button 1
if delta[0] == 1 and self.state != Controller.Automatic:
print("Automatic!")
thrust = self.cmd_vel_telop.linear.z
print(thrust)
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.state = Controller.TakeOff
print("TakeOff!")
#Button 5
if delta[4] == 1:
self.targetZ += 0.1
print(self.targetZ)
#Button 6
if delta[5] == 1:
self.targetZ -= 0.1
print(self.targetZ)
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark")
if self.listener.canTransform("/mocap", "/Nano_Mark", t):
position, quaternion = self.listener.lookupTransform("/mocap", "/Nano_Mark", t)
if position[2] > 0.1 or thrust > 50000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap")
print(t);
if self.listener.canTransform("/Nano_Mark", "/mocap", t):
targetWorld = PoseStamped()
targetWorld.header.stamp = t
targetWorld.header.frame_id = "mocap"
targetWorld.pose.position.x = 0
targetWorld.pose.position.y = 0
targetWorld.pose.position.z = self.targetZ
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
targetWorld.pose.orientation.x = quaternion[0]
targetWorld.pose.orientation.y = quaternion[1]
targetWorld.pose.orientation.z = quaternion[2]
targetWorld.pose.orientation.w = quaternion[3]
targetDrone = self.listener.transformPose("/Nano_Mark", targetWorld)
quaternion = (
targetDrone.pose.orientation.x,
targetDrone.pose.orientation.y,
targetDrone.pose.orientation.z,
targetDrone.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
#msg = self.cmd_vel_teleop
msg.linear.x = self.pidX.update(0.0, targetDrone.pose.position.x)
msg.linear.y = self.pidY.update(0.0, targetDrone.pose.position.y)
msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) #self.pidZ.update(position[2], self.targetZ)
msg.angular.z = self.pidYaw.update(0.0, euler[2])
#print(euler[2])
#print(msg.angular.z)
self.pubNav.publish(msg)
rospy.sleep(0.01)
class ur5_grasp_project(object):
def __init__(self, joint_values=None):
rospy.init_node('command_GGCNN_ur5')
self.joint_values_home = joint_values
self.tf = TransformListener()
# Used to change the controller
self.controller_switch = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
# actionClient used to send joint positions
self.client = actionlib.SimpleActionClient(
'pos_based_pos_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
print("Waiting for server (pos_based_pos_traj_controller)...")
self.client.wait_for_server()
print("Connected to server (pos_based_pos_traj_controller)")
#################
# GGCNN Related
#################
self.d = None
self.ori_ = None
self.grasp_cartesian_pose = None
self.posCB = []
self.ori = []
self.gripper_angle_grasp = 0.0
self.final_orientation = 0.0
# These offsets are used only in the real robot and need to be calibrated
self.GGCNN_offset_x = -0.03 # 0.002
self.GGCNN_offset_y = 0.02 # -0.05
self.GGCNN_offset_z = 0.058 # 0.013
self.robotiq_joint_name = rospy.get_param("/robotiq_joint_name")
# Topic published from GG-CNN Node
rospy.Subscriber('ggcnn/out/command',
Float32MultiArray,
self.ggcnn_command_callback,
queue_size=1)
####################
# Pipeline Related #
####################
self.classes = rospy.get_param("/classes")
self.grasp_ready_flag = False
self.detected_tags = []
self.tags = [
'tag_0_corrected', 'tag_1_corrected', 'tag_2_corrected',
'tag_3_corrected', 'tag_4_corrected', 'tag_5_corrected',
'tag_6_corrected', 'tag_7_corrected'
]
# These offset for the real camera
self.tags_position_offset = [0.062, 0.0, 0.062]
# Subscribers - Topics related to the new grasping pipeline
rospy.Subscriber('flags/grasp_ready',
Bool,
self.grasp_ready_callback,
queue_size=1) # Grasp flag
rospy.Subscriber('flags/reposition_robot_flag',
Bool,
self.reposition_robot_callback,
queue_size=1) # Reposition flag
rospy.Subscriber('flags/detection_ready',
Bool,
self.detection_ready_callback,
queue_size=1) # Detection flag
rospy.Subscriber('reposition_coord',
Float32MultiArray,
self.reposition_coord_callback,
queue_size=1)
rospy.Subscriber('/selective_grasping/tag_detections',
Int16MultiArray,
self.tags_callback,
queue_size=1)
# Publishers
# Publish the required class so the other nodes such as gg-cnn generates the grasp to the selected part
self.required_class = rospy.Publisher('pipeline/required_class',
Int8,
queue_size=1)
def turn_velocity_controller_on(self):
self.controller_switch(['joint_group_vel_controller'],
['pos_based_pos_traj_controller'], 1)
def turn_position_controller_on(self):
self.controller_switch(['pos_based_pos_traj_controller'],
['joint_group_vel_controller'], 1)
def turn_gripper_velocity_controller_on(self):
self.controller_switch(['gripper_controller_vel'],
['gripper_controller_pos'], 1)
def turn_gripper_position_controller_on(self):
self.controller_switch(['gripper_controller_pos'],
['gripper_controller_vel'], 1)
def tags_callback(self, msg):
self.detected_tags = msg.data
def grasp_ready_callback(self, msg):
self.grasp_ready_flag = msg.data
def detection_ready_callback(self, msg):
self.detection_ready_flag = msg.data
def reposition_robot_callback(self, msg):
self.reposition_robot_flag = msg.data
def reposition_coord_callback(self, msg):
self.reposition_coords = msg.data
def ur5_actual_position_callback(self, joint_values_from_ur5):
"""Get UR5 joint angles
The joint states published by /joint_states of the UR5 robot are in wrong order.
/joint_states topic normally publishes the joint in the following order:
[elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
But the correct order of the joints that must be sent to the robot is:
['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Arguments:
joint_values_from_ur5 {list} -- Actual angles of the UR5 Robot
"""
self.th3, self.th2, self.th1, self.th4, self.th5, self.th6 = joint_values_from_ur5.position
self.actual_position = [
self.th1, self.th2, self.th3, self.th4, self.th5, self.th6
]
def genCommand(self, char, command, pos=None):
"""
Update the command according to the character entered by the user.
"""
if char == 'a':
# command = outputMsg.Robotiq2FGripper_robot_output();
command.rACT = 1 # Gripper activation
command.rGTO = 1 # Go to position request
command.rSP = 255 # Speed
command.rFR = 150 # Force
if char == 'r':
command.rACT = 0
if char == 'c':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = 255
command.rSP = 40
command.rFR = 150
# @param pos Gripper width in meters. [0, 0.087]
if char == 'p':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = int(
np.clip(
(13. - 230.) / self.gripper_max_width * self.ori + 230., 0,
255))
command.rSP = 40
command.rFR = 150
if char == 'o':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = 0
command.rSP = 40
command.rFR = 150
return command
def command_gripper(self, action):
command = outputMsg.Robotiq2FGripper_robot_output()
command = self.genCommand(action, command)
self.pub_gripper_command.publish(command)
def ggcnn_command_callback(self, msg):
"""
GGCNN Command Subscriber Callback
"""
self.tf.waitForTransform("base_link", "object_detected", rospy.Time(0),
rospy.Duration(4.0)) # rospy.Time.now()
object_pose, object_ori = self.tf.lookupTransform(
"base_link", "object_detected", rospy.Time(0))
self.d = list(msg.data)
object_pose[0] += self.GGCNN_offset_x
object_pose[1] += self.GGCNN_offset_y
object_pose[2] += self.GGCNN_offset_z
self.posCB = object_pose
self.ori = self.d[3]
br = TransformBroadcaster()
br.sendTransform((object_pose[0], object_pose[1], object_pose[2]),
quaternion_from_euler(0.0, 0.0, self.ori),
rospy.Time.now(), "object_link", "base_link")
def all_close(self, goal, tolerance=0.00005):
"""Wait until goal is reached in configuration space
This method check if the robot reached goal position since wait_for_result seems to be broken
Arguments:
goal {[list]} -- Goal in configuration space (joint values)
Keyword Arguments:
tolerance {number} -- Minimum error allowed to consider the trajectory completed (default: {0.00005})
"""
error = np.sum([(self.actual_position[i] - goal[i])**2
for i in range(6)])
print("> Waiting for the trajectory to finish...")
while not rospy.is_shutdown() and error > tolerance:
error = np.sum([(self.actual_position[i] - goal[i])**2
for i in range(6)])
if error < tolerance:
print(
"> Trajectory Suceeded!\n") # whithin the tolerance specified
else:
rospy.logerr("> Trajectory aborted.")
def traj_planner(self,
grasp_position,
grasp_step='move',
way_points_number=10,
movement='slow'):
"""Quintic Trajectory Planner
Publish a trajectory to UR5 using quintic splines.
Arguments:
grasp_position {[float]} -- Grasp position [x, y, z]
Keyword Arguments:
grasp_step {str} -- Set UR5 movement type (default: {'move'})
way_points_number {number} -- Number of points considered in trajectory (default: {10})
movement {str} -- Movement speed (default: {'slow'})
"""
if grasp_step == 'pregrasp':
# we need to take a 'screenshot' of the variables at this specific time
self.grasp_cartesian_pose = deepcopy(self.posCB)
self.ori_ = deepcopy(self.ori)
grasp_cartesian_pose = self.grasp_cartesian_pose
posCB = self.posCB
ori = self.ori_
actual_position = self.actual_position
d = self.d
status, goal, joint_pos = IK_TRAJ.traj_planner(
grasp_position, grasp_step, way_points_number, movement,
grasp_cartesian_pose, ori, d, actual_position)
if status:
self.client.send_goal(goal)
self.all_close(joint_pos)
else:
print("Could not retrieve any IK solution")
def move_on_shutdown(self):
self.client.cancel_goal()
self.client_gripper.cancel_goal()
print("Shutting down node...")
def grasp_main(self, point_init_home, depth_shot_point):
random_classes = [i for i in range(len(self.classes))]
bin_location = [[-0.65, -0.1, 0.2], [-0.65, 0.1, 0.2]]
garbage_location = [-0.4, -0.30, 0.10]
raw_input('=== Press enter to start the grasping process')
while not rospy.is_shutdown():
# The grasp is performed randomly and the chosen object is published to
# the detection node
if not len(random_classes):
print(
"\n> IMPORTANT! There is no object remaining in the object's picking list. Resetting the objects list...\n"
)
random_classes = [i for i in range(len(self.classes))]
random_object_id = random.sample(random_classes, 1)[0]
random_classes.pop(random_classes.index(random_object_id))
print('> Object to pick: ', self.classes[random_object_id])
# Publish the required class ID so the other nodes such as gg-cnn
# generates the grasp to the selected part
self.required_class.publish(random_object_id)
raw_input("==== Press enter to start the grasping process!")
if self.detection_ready_flag:
# Check if the robot needs to be repositioned to reach the object. This flag is published
# By the detection node. The offset coordinates (self.reposition_coords) are also published
# by the detection node
if self.reposition_robot_flag:
print('> Repositioning robot...')
self.tf.waitForTransform("base_link", "grasping_link",
rospy.Time.now(),
rospy.Duration(4.0))
eef_pose, _ = self.tf.lookupTransform(
"base_link", "grasping_link", rospy.Time(0))
corrected_position = [
eef_pose[0] - self.reposition_coords[1],
eef_pose[1] + self.reposition_coords[0], eef_pose[2]
]
self.traj_planner(corrected_position, movement='fast')
raw_input("==== Press enter when the trajectory finishes!")
# Check if:
# - The run_ggcn is ready (through grasp_ready_flag);
# - The detection node is ready (through detection_ready_flag);
# - The robot does not need to be repositioned (through reposition_robot_flag)
if self.grasp_ready_flag and self.detection_ready_flag and not self.reposition_robot_flag:
raw_input("Move to the pre grasp position")
print('> Moving to the grasping position... \n')
self.traj_planner([], 'pregrasp', movement='fast')
raw_input("Move the gripper")
# It closes the gripper a little before approaching the object
# to prevent colliding with other objects
self.command_gripper('p')
raw_input("Move to the grasp position")
# Moving the robot to pick the object - BE CAREFULL!
self.traj_planner([], 'grasp', movement='slow')
raw_input("==== Press enter to close the gripper!")
print("Picking object...")
self.command_gripper('c')
# After a collision is detected, the arm will start the picking action
# Different locations were adopted because the camera field of view is not big enough
# to detect all the april tags in the environment
raw_input("==== Press enter to move the object to the bin")
if random_object_id < 5:
self.traj_planner(bin_location[0], movement='fast')
else:
self.traj_planner(bin_location[1], movement='fast')
rospy.sleep(2.0) # waiting for the tag detections
selected_tag_status = self.detected_tags[random_object_id]
print('selected tag: ', self.tags[random_object_id])
# Check if the tag corresponding to the object ID was identified in order to
# move the object there
if selected_tag_status:
self.tf.waitForTransform(
"base_link", self.tags[random_object_id],
rospy.Time(0),
rospy.Duration(2.0)) # rospy.Time.now()
ptFinal, oriFinal = self.tf.lookupTransform(
"base_link", self.tags[random_object_id],
rospy.Time(0))
ptFinal = [
ptFinal[i] + self.tags_position_offset[i]
for i in range(3)
]
raw_input("==== Move to the tag")
pt_inter = deepcopy(ptFinal)
# pt_inter adds 0.1m to the Z coordinate of ptFinal in order to
# approach the box before leaving the object there
pt_inter[-1] += 0.1
self.traj_planner(pt_inter, movement='fast')
self.traj_planner(ptFinal, movement='fast')
else:
# In this case, the camera identifies some other tag but not the tag corresponding to the object
print(
'> Could not find the box tag. Placing the object anywhere \n'
)
raw_input('=== Pres enter to proceed')
self.traj_planner(garbage_location, movement='fast')
print("Placing object...")
# After the bin location is reached, the robot will place the object and move back
# to the initial position
self.command_gripper('o')
print("> Moving back to home position...\n")
self.traj_planner(point_init_home, movement='fast')
self.traj_planner(depth_shot_point, movement='slow')
print('> Grasping finished \n')
else:
print('> The requested object could not be grasped! \n')
self.traj_planner(depth_shot_point, movement='fast')
else:
print('> The requested object was not detected! \n')
class PickAndPlace(object):
def __init__(self, limb, start_pose, hover_distance=0.15):
self.start_pose = start_pose
self.hover_distance = hover_distance
self.limb = robot_limb.Limb(limb)
self.gripper = robot_gripper.Gripper(limb)
self.base = "base_marker"
self.pick_obj = "obj_marker"
self.dest = "dest_marker"
self.destinations = []
self.queue = Queue.Queue()
self.tf = TransformListener()
self.set_scene()
self.set_limb_planner()
self.enable_robot()
self.set_destinations()
print("Moving Sawyer Arm to Start Position")
self.move_to_start()
def set_limb_planner():
self.right_arm = MoveGroupCommander("right_arm")
self.right_arm.set_planner_id('RRTConnectkConfigDefault')
self.right_arm.allow_replanning(True)
self.right_arm.set_planning_time(7)
def enable_robot():
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def set_scene():
self.scene_pub = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
self.scene = PlanningSceneInterface()
rospy.sleep(2)
self.add_collision_object('turtlebot', 0.8, 0., -0.65, .5, 1.5, .33)
self.add_collision_object('right_wall', 0., 0.65, 0., 4., .1, 4.)
self.add_collision_object('left_wall', 0., -0.8, 0., 4., .1, 4.)
self.add_collision_object('back_wall', -0.6, 0., 0., .1, 4., 4.)
self.add_collision_object('destination_table', .5, -.4, -.35, 1, .5,
.5)
self.plan_scene = PlanningScene()
self.plan_scene.is_diff = True
self.scene_pub.publish(self.plan_scene)
def add_collision_object(self, name, x, y, z, xs, ys, zs):
self.scene.remove_world_object(name)
o = PoseStamped()
o.pose.position.x = x
o.pose.position.y = y
o.pose.position.z = z
self.scene.attach_box('base', name, o, [xs, ys, zs])
def set_destinations(self):
while len(self.destinations) == 0:
if self.tf.frameExists(self.base) and self.tf.frameExists(
self.dest):
point, quaternion = self.tf.lookupTransform(
self.base, self.dest,
self.tf.getLatestCommonTime(self.base, self.dest))
position = list_to_point(point)
self.destinations.append(
Pose(position=position,
orientation=self.start_pose.orientation))
def add_new_objects_to_queue(self, sleep=True):
print("Checking if " + self.base + " and " + self.pick_obj +
" both exist.")
if self.tf.frameExists(self.base) and self.tf.frameExists(
self.pick_obj):
if sleep:
rospy.sleep(10.0)
self.add_new_objects_to_queue(sleep=False)
else:
print(self.base + " and " + self.pick_obj + " both exist.")
point, quaternion = self.tf.lookupTransform(
self.base, self.pick_obj,
self.tf.getLatestCommonTime(self.base, self.pick_obj))
position = list_to_point(point)
print("Adding " + self.pick_obj + " to queue")
obj_location = Pose(position=position,
orientation=self.start_pose.orientation)
print("Picking Object from:", obj_location)
self.queue.put(obj_location)
def complete_pick_place(self):
if not self.queue.empty():
start_pose = self.queue.get()
end_pose = self.destinations[0]
print("\nPicking...")
self.pick(start_pose)
print("\nPlacing...")
self.place(end_pose)
print("\Resetting...")
self.move_to_start()
def guarded_move_to_joint_position(self, pose):
self.right_arm.set_pose_target(pose)
self.right_arm.go()
def servo_to_pose(self, pose):
# servo down to release
self.guarded_move_to_joint_position(pose)
def approach(self, pose):
# approach with a pose the hover-distance above the requested pose
approach = copy.deepcopy(pose)
approach.position.z += self.hover_distance
print("approaching:", approach)
self.guarded_move_to_joint_position(approach)
def retract(self):
# retrieve current pose from endpoint
current_pose = self.limb.endpoint_pose()
ik_pose = Pose()
ik_pose.position.x = current_pose['position'].x
ik_pose.position.y = current_pose['position'].y
ik_pose.position.z = current_pose['position'].z + self.hover_distance
ik_pose.orientation.x = current_pose['orientation'].x
ik_pose.orientation.y = current_pose['orientation'].y
ik_pose.orientation.z = current_pose['orientation'].z
ik_pose.orientation.w = current_pose['orientation'].w
# servo up from current pose
self.guarded_move_to_joint_position(ik_pose)
def move_to_start(self):
print("Moving the right arm to start pose...")
self.guarded_move_to_joint_position(self.start_pose)
self.gripper_open()
rospy.sleep(1.0)
print("Running. Ctrl-c to quit")
def gripper_open(self):
self.gripper.open()
rospy.sleep(1.0)
def gripper_close(self):
self.gripper.close()
rospy.sleep(1.0)
def pick(self, pose):
# open the gripper
self.gripper_open()
# servo above pose
self.approach(pose)
# servo to pose
self.servo_to_pose(pose)
# close gripper
self.gripper_close()
# retract to clear object
self.retract()
def place(self, pose):
# servo above pose
self.approach(pose)
# servo to pose
self.servo_to_pose(pose)
# open the gripper
self.gripper_open()
# retract to clear object
self.retract()
class DriveForwardAction():
"""
Drives x meters either forward or backwards depending on the given distance.
the velocity should always be positive.
"""
def __init__(self,name,odom_frame,base_frame):
"""
@param name: the name of the action
@param odom_frame: the frame the robot is moving in (odom_combined)
@param base_frame: the vehicles own frame (usually base_link)
"""
self._action_name = name
self.__odom_frame = odom_frame
self.__base_frame = base_frame
self.__server = actionlib.SimpleActionServer(self._action_name,drive_forwardAction,auto_start=False)
self.__server.register_preempt_callback(self.preempt_cb)
self.__server.register_goal_callback(self.goal_cb)
self.__cur_pos = 0
self.__start_yaw = 0
self.__cur_yaw = 0
self.__feedback = drive_forwardFeedback()
self.__listen = TransformListener()
self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self.__turn_timeout = 200
self.__start_time = rospy.Time.now()
self.turn_vel = 0
self.new_goal = False
self.fix_offset = 0
self.__server.start()
def preempt_cb(self):
rospy.loginfo("Preempt requested")
self.__publish_cmd_vel(0)
self.__server.set_preempted()
def goal_cb(self):
"""
called when we receive a new goal
the goal contains a desired radius and a success radius in which we check if the turn succeeded or not
the message also contains if we should turn left or right
"""
g = self.__server.accept_new_goal()
self.__desired_amount= g.amount
self.vel = g.vel
self.fix_offset = g.fix_offset
self.new_goal = True
def on_timer(self,e):
"""
called regularly by a ros timer
in here we simply orders the vehicle to start moving either forward
or backwards depending on the distance sign, while monitoring the
travelled distance, if the distance is equal to or greater then this
action succeeds.
"""
if self.__server.is_active():
if self.new_goal:
self.new_goal = False
if self.__get_start_position():
self.__start_time = rospy.Time.now()
else:
self.__server.set_aborted(text="could not find vehicle")
else:
if rospy.Time.now() - self.__start_time > rospy.Duration(self.__turn_timeout):
self.__server.set_aborted(text="timeout on action")
self.__publish_cmd_vel(0)
else:
if self.__get_current_position():
if self.__get_distance() >= math.fabs(self.__desired_amount):
result = drive_forwardResult()
result.end_dist = self.__get_distance()
self.__server.set_succeeded(result, "distance covered")
self.__publish_cmd_vel(0)
else:
self.__publish_cmd_vel(1)
self.__feedback.progress = self.__get_distance()
self.__server.publish_feedback(self.__feedback)
def __get_distance(self):
return math.sqrt(math.pow(self.__cur_pos[0][0] - self.__start_pos[0][0],2) +
math.pow(self.__cur_pos[0][1] - self.__start_pos[0][1],2))
def __get_start_position(self):
ret = False
try:
self.__start_pos = self.__listen.lookupTransform(self.__odom_frame,self.__base_frame,rospy.Time(0))
self.__start_yaw = tf.transformations.euler_from_quaternion(self.__start_pos[1])[2]
ret = True
except (tf.LookupException, tf.ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
return ret
class Drone:
def __init__(self, name, obstacles, leader=False):
self.name = name
self.tf = '/vicon/' + name + '/' + name
self.leader = leader
self.tl = TransformListener()
self.pose = self.position()
self.orient = np.array([0, 0, 0])
self.sp = self.position()
self.obstacles = obstacles
self.path = Path()
self.delta = np.array([0, 0])
self.near_obstacle = np.zeros(len(obstacles))
self.radius_impedance = Radius_Impedance()
self.angular_impedance = Angular_Impedance()
self.impedance_avel = Impedance_avel()
self.theta = None
self.d_theta = pi * np.ones(len(obstacles))
self.dist_to_drones = np.zeros(3)
self.dist_to_obst = np.zeros(len(obstacles))
self.drone_time_array = np.ones(10)
self.drone_pose_array = np.array(
[np.ones(10), np.ones(10), np.ones(10)])
self.rate = rospy.Rate(100)
self.traj = np.array([0, 0, 0])
self.feature = self.sp
self.start = self.position()
self.goal = self.sp
def position(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.pose = position
return np.array(position)
def orientation(self):
self.tl.waitForTransform("/world", self.tf, rospy.Time(0),
rospy.Duration(1))
position, quaternion = self.tl.lookupTransform("/world", self.tf,
rospy.Time(0))
self.orient = get_angles(np.array(quaternion))
return get_angles(np.array(quaternion))
def publish_sp(self):
publish_pose(self.sp, np.array([0, 0, 0]), self.name + "_sp")
def publish_position(self):
publish_pose(self.pose, self.orient, self.name + "_pose")
def publish_path(self, limit=1000):
publish_path(self.path, self.sp, self.orient, self.name + "_path",
limit)
#for i in range( 1,len(self.path.poses) ):
# print self.name +": "+ str(self.path.poses[i].pose)
def fly(self):
# if self.leader:
# limits = np.array([ 2, 2, 2.5 ])
# np.putmask(self.sp, self.sp >= limits, limits)
# np.putmask(self.sp, self.sp <= -limits, -limits)
publish_goal_pos(self.sp, 0, "/" + self.name)
def update_pose_theta(self, drone, obstacle, R):
drone_omega = self.omegaZ(drone.sp, drone.sp - obstacle.position())
if drone.near_obstacle[
obstacle.
id] == 1: # initial impedance parameters, when the drone is just near the obstacle
drone.angular_impedance.imp_theta, drone.angular_impedance.imp_omega, drone.angular_impedance.imp_time =\
drone.angular_impedance.impedance_model(drone.theta, drone.theta, drone_omega, drone.angular_impedance.imp_time)
else:
drone.angular_impedance.imp_theta, drone.angular_impedance.imp_omega, drone.angular_impedance.imp_time =\
drone.angular_impedance.impedance_model(drone.theta, drone.angular_impedance.imp_theta, drone.angular_impedance.imp_omega, drone.angular_impedance.imp_time)
updated_pose = drone.angular_impedance.pose_from_theta(
obstacle.position()[:2], R, drone.angular_impedance.imp_theta)
return updated_pose
def update_pose_radius(self, pose_prev, drone, dist_to_obstacle, koef):
if (
sum(drone.near_obstacle) <= 10
): # 10 is the number of samples to compute velocity from consequent poses
drone.radius_impedance.imp_vel = drone.velocity(drone.sp)
drone.radius_impedance.imp_pose, \
drone.radius_impedance.imp_vel, \
drone.radius_impedance.imp_time = \
drone.radius_impedance.impedance_model(drone.delta, drone.radius_impedance.imp_pose, drone.radius_impedance.imp_vel, drone.radius_impedance.imp_time)
delta_pose = drone.radius_impedance.imp_pose[:2]
updated_pose = pose_prev + koef * delta_pose
''' impedance model vizualization '''
length = np.linalg.norm(delta_pose)
yaw = acos(delta_pose[0] / length)
publish_arrow(np.hstack([updated_pose, drone.sp[2]]), [0, 0, yaw],
length, '/delta_pose')
return updated_pose
def update_pose_avel(self, pose_prev, average_vel):
self.impedance_avel.imp_pose, self.impedance_avel.imp_vel, self.impedance_avel.imp_time = \
self.impedance_avel.impedance_model(average_vel, self.impedance_avel.imp_pose, self.impedance_avel.imp_vel, self.impedance_avel.imp_time)
updated_pose = pose_prev + 0.15 * self.impedance_avel.imp_pose
return updated_pose
def calculate_dist_to_drones(self, drones_poses):
for i in range(len(drones_poses)):
self.dist_to_drones[i] = np.linalg.norm(drones_poses[i] - self.sp)
def dist_to_obstacles(self):
dist = []
for i in range(len(self.obstacles)):
dist.append(
np.linalg.norm(self.obstacles[i].position()[:2] - self.sp[:2]))
self.dist_to_obst = dist
return np.array(dist)
def omegaZ(self, drone_pose, dist_from_obstacle):
R = dist_from_obstacle # 3D-vector
drone_vel = self.velocity(drone_pose)
# drone_vel_n = np.dot(drone_vel, R)/(np.linalg.norm(R)**2) * R
# drone_vel_t = drone_vel - drone_vel_n
drone_w = np.cross(R, drone_vel) # / ( np.linalg.norm(R)**2 )
return drone_w[2]
def velocity(self, drone_pose):
for i in range(len(self.drone_time_array) - 1):
self.drone_time_array[i] = self.drone_time_array[i + 1]
self.drone_time_array[-1] = time.time()
for i in range(len(self.drone_pose_array[0]) - 1):
self.drone_pose_array[0][i] = self.drone_pose_array[0][i + 1]
self.drone_pose_array[1][i] = self.drone_pose_array[1][i + 1]
self.drone_pose_array[2][i] = self.drone_pose_array[2][i + 1]
self.drone_pose_array[0][-1] = drone_pose[0]
self.drone_pose_array[1][-1] = drone_pose[1]
self.drone_pose_array[2][-1] = drone_pose[2]
vel_x = (self.drone_pose_array[0][-1] - self.drone_pose_array[0][0]
) / (self.drone_time_array[-1] - self.drone_time_array[0])
vel_y = (self.drone_pose_array[1][-1] - self.drone_pose_array[1][0]
) / (self.drone_time_array[-1] - self.drone_time_array[0])
vel_z = (self.drone_pose_array[2][-1] - self.drone_pose_array[2][0]
) / (self.drone_time_array[-1] - self.drone_time_array[0])
drone_vel = np.array([vel_x, vel_y, vel_z])
return drone_vel
def landing(self, sim=False):
drone_landing_pose = self.position() if sim == False else self.sp
while not rospy.is_shutdown():
self.sp = drone_landing_pose
drone_landing_pose[2] = drone_landing_pose[2] - 0.007
self.publish_sp()
self.publish_path(limit=1000)
if sim == False:
self.fly()
if self.sp[2] < -1.0:
sleep(1)
if sim == False:
cf1.stop()
print 'reached the floor, shutdown'
# rospy.signal_shutdown('landed')
self.rate.sleep()
class PositionPlanner:
"""
Control class taking position goals and generating twist messages accordingly
"""
def __init__(self):
# Init control methods
self.isNewGoalAvailable = self.empty_method()
self.isPreemptRequested = self.empty_method()
self.setPreempted = self.empty_method()
# Get topics and transforms
self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/fmSignals/cmd_vel")
self.odom_frame = rospy.get_param("~odom_frame", "/odom")
self.odometry_topic = rospy.get_param("~odometry_topic", "/fmKnowledge/odom")
self.base_frame = rospy.get_param("~base_frame", "/base_footprint")
self.use_tf = rospy.get_param("~use_tf", False)
# Get general parameters
self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
self.max_initial_error = rospy.get_param("~max_initial_angle_error", 1)
self.max_distance_error = rospy.get_param("~max_distance_error", 0.2)
self.use_tf = rospy.get_param("~use_tf", False)
self.max_angle_error = rospy.get_param("~max_angle_error", math.pi / 4)
self.retarder = rospy.get_param("~retarder", 0.8)
# Control loop
self.lin_p = rospy.get_param("~lin_p", 0.4)
self.lin_i = rospy.get_param("~lin_i", 0.6)
self.lin_d = rospy.get_param("~lin_d", 0.0)
self.ang_p = rospy.get_param("~ang_p", 0.8)
self.ang_i = rospy.get_param("~ang_i", 0.1)
self.ang_d = rospy.get_param("~ang_d", 0.05)
self.int_max = rospy.get_param("~int_max", 0.1)
# Setup Publishers and subscribers
if not self.use_tf:
self.odometry_topic = rospy.get_param("~odometry_topic", "/fmKnowledge/odom")
self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry, self.onOdometry)
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
# Parameters for action server
self.period = 0.1
self.retarder_point = 0.3 # distance to target when speed should start declining
# Init control loop
self.lin_err = 0.0
self.ang_err = 0.0
self.lin_prev_err = 0.0
self.ang_prev_err = 0.0
self.lin_int = 0.0
self.ang_int = 0.0
self.lin_diff = 0.0
self.ang_diff = 0.0
self.distance_error = 0
self.angle_error = 0
self.fb_linear = 0.0
self.fb_angular = 0.0
self.sp_linear = 0.0
self.sp_angular = 0.0
# Init TF listener
self.__listen = TransformListener()
# Init controller
self.corrected = False
self.rate = rospy.Rate(1 / self.period)
self.twist = TwistStamped()
self.destination = Vector(0, 0)
self.position = Vector(0, 0)
self.heading = Vector(0, 0)
self.quaternion = np.empty((4,), dtype=np.float64)
# Setup Publishers and subscribers
self.use_tf = False
if not self.use_tf:
self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry, self.onOdometry)
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
def execute(self, goal):
# Construct a vector from position goal
self.destination[0] = goal.x
self.destination[1] = goal.y
rospy.loginfo(rospy.get_name() + "Received goal: (%f,%f) ", goal.x, goal.y)
self.corrected = False
while not rospy.is_shutdown():
# Check for new goal
if self.isNewGoalAvailable():
break
# Preemption check
if self.isPreemptRequested():
break
# If position is unreached, publish twist
if self.publish_twist(self.destination, self.destination):
# Block
try:
self.rate.sleep()
except rospy.ROSInterruptException:
return "preempted"
else:
# Succeed the action - position has been reached
self.setSucceeded()
self.stop()
break
# Return state
if self.isPreemptRequested():
self.setPreempted()
return "preempted"
elif rospy.is_shutdown():
return "aborted"
else:
return "succeeded"
def stop(self):
# Publish a zero twist to stop the robot
self.twist.header.stamp = rospy.Time.now()
self.twist.twist.linear.x = 0
self.twist.twist.angular.z = 0
self.twist_pub.publish(self.twist)
def publish_twist(self, target, goal):
"""
Method running the control loop. Distinguishes between target and goal to
adapt to other planners. For position planning the two will be the same.
"""
# Get current position
self.get_current_position()
# Construct goal path vector
goal_path = goal - Vector(self.position[0], self.position[1])
# Calculate distance to goal
self.distance_error = goal_path.length()
# If the goal is unreached
if self.distance_error > self.max_distance_error:
# Construct target path vector
target_path = target - Vector(self.position[0], self.position[1])
# Calculate yaw
if self.use_tf:
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(self.heading)
else:
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(self.quaternion)
# Construct heading vector
head = Vector(math.cos(yaw), math.sin(yaw))
# Calculate angle between heading vector and target path vector
self.angle_error = head.angle(target_path)
# Rotate the heading vector according to the calculated angle and test correspondence
# with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
t1 = head.rotate(self.angle_error)
if target_path.angle(t1) != 0:
self.angle_error = -self.angle_error
# Calculate angle between heading vector and goal path vector
goal_angle_error = head.angle(goal_path)
# Avoid the sine trap.
t1 = head.rotate(goal_angle_error)
if goal_path.angle(t1) != 0:
goal_angle_error = -goal_angle_error
# Check if large initial errors have been corrected
if math.fabs(self.angle_error) < self.max_initial_error:
self.corrected = True
# Generate velocity setpoints from distance and angle errors
self.sp_linear = self.distance_error
self.sp_angular = self.angle_error
# Calculate velocity errors for control loop
self.lin_err = self.sp_linear - self.fb_linear
self.ang_err = self.sp_angular - self.fb_angular
# Calculate integrators and implement max
self.lin_int += self.lin_err * self.period
if self.lin_int > self.int_max:
self.lin_int = self.int_max
if self.lin_int < -self.int_max:
self.lin_int = -self.int_max
self.ang_int += self.ang_err * self.period
if self.ang_int > self.int_max:
self.ang_int = self.int_max
if self.ang_int < -self.int_max:
self.ang_int = -self.int_max
# Calculate differentiators and save value
self.lin_diff = (self.lin_prev_err - self.lin_err) / self.period
self.ang_diff = (self.ang_prev_err - self.ang_err) / self.period
self.lin_prev_err = self.lin_err
self.ang_prev_err = self.ang_err
# Update twist message with control velocities
self.twist.twist.linear.x = (
(self.lin_err * self.lin_p) + (self.lin_int * self.lin_i) + (self.lin_diff * self.lin_d)
)
self.twist.twist.angular.z = (
(self.ang_err * self.ang_p) + (self.ang_int * self.ang_i) + (self.ang_diff * self.ang_d)
)
# Implement retarder to reduce linear velocity if angle error is too big
if math.fabs(goal_angle_error) > self.max_angle_error:
self.twist.twist.linear.x *= self.retarder
# Implement initial correction speed for large angle errors
if not self.corrected:
self.twist.twist.linear.x *= self.retarder ** 2
# Implement maximum linear velocity and maximum angular velocity
if self.twist.twist.linear.x > self.max_linear_velocity:
self.twist.twist.linear.x = self.max_linear_velocity
if self.twist.twist.linear.x < -self.max_linear_velocity:
self.twist.twist.linear.x = -self.max_linear_velocity
if self.twist.twist.angular.z > self.max_angular_velocity:
self.twist.twist.angular.z = self.max_angular_velocity
if self.twist.twist.angular.z < -self.max_angular_velocity:
self.twist.twist.angular.z = -self.max_angular_velocity
# Prohibit reverse driving
if self.twist.twist.linear.x < 0:
self.twist.twist.linear.x = 0
# If not preempted, add a time stamp and publish the twist
if not self.isPreemptRequested():
self.twist.header.stamp = rospy.Time.now()
self.twist_pub.publish(self.twist)
return True
else:
return False
def onOdometry(self, msg):
"""
Callback method for handling odometry messages
"""
# Extract the orientation quaternion
self.quaternion[0] = msg.pose.pose.orientation.x
self.quaternion[1] = msg.pose.pose.orientation.y
self.quaternion[2] = msg.pose.pose.orientation.z
self.quaternion[3] = msg.pose.pose.orientation.w
# Extract the position vector
self.position[0] = msg.pose.pose.position.x
self.position[1] = msg.pose.pose.position.y
# Extract twist
self.fb_linear = msg.twist.twist.linear.x
self.fb_angular = msg.twist.twist.angular.z
def empty_method(self):
"""
Empty method pointer
"""
return False
def get_current_position(self):
"""
Get current position from tf
"""
if self.use_tf:
ret = False
try:
(self.position, self.heading) = self.__listen.lookupTransform(
self.odom_frame, self.base_frame, rospy.Time(0)
) # The transform is returned as position (x,y,z) and an orientation quaternion (x,y,z,w).
ret = True
except (tf.LookupException, tf.ConnectivityException), err:
rospy.loginfo("could not locate vehicle")
return ret
class SprayAction():
"""
Drives x meters either forward or backwards depending on the given distance.
the velocity should always be positive.
"""
def __init__(self, name, odom_frame, base_frame):
"""
@param name: the name of the action
@param odom_frame: the frame the robot is moving in (odom_combined)
@param base_frame: the vehicles own frame (usually base_link)
"""
self._action_name = name
self.__odom_frame = odom_frame
self.__base_frame = base_frame
self.__server = actionlib.SimpleActionServer(self._action_name,
sprayAction,
auto_start=False)
self.__server.register_preempt_callback(self.preempt_cb)
self.__server.register_goal_callback(self.goal_cb)
self.__cur_pos = 0
self.__start_pos = 0
self.__start_yaw = 0
self.__cur_yaw = 0
self.__feedback = sprayFeedback()
self.__listen = TransformListener()
#self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self.spray_pub = rospy.Publisher("/fmControllers/spray", UInt32)
self.__start_time = rospy.Time.now()
self.new_goal = False
self.n_sprays = 0
self.spray_msg = UInt32()
self.spray_l = False
self.spray_cnt = 0
self.__server.start()
def preempt_cb(self):
rospy.loginfo("Preempt requested")
self.spray_msg.data = 0
self.spray_pub.publish(self.spray_msg)
self.__server.set_preempted()
def goal_cb(self):
"""
called when we receive a new goal
the goal contains a desired radius and a success radius in which we check if the turn succeeded or not
the message also contains if we should turn left or right
"""
g = self.__server.accept_new_goal()
self.spray_dist = g.distance
self.spraytime = g.spray_time
self.new_goal = True
def on_timer(self, e):
"""
called regularly by a ros timer
in here we simply orders the vehicle to start moving either forward
or backwards depending on the distance sign, while monitoring the
travelled distance, if the distance is equal to or greater then this
action succeeds.
"""
if self.__server.is_active():
if self.new_goal:
self.new_goal = False
self.n_sprays = 0
self.__get_start_position()
else:
if self.__get_current_position():
if self.__get_distance() >= self.spray_dist:
self.do_spray()
self.__get_start_position()
else:
self.__server.set_aborted(None, "could not locate")
rospy.logerr("Could not locate vehicle")
self.spray_msg.data = 0
self.spray_pub.publish(self.spray_msg)
def __get_distance(self):
return math.sqrt(
math.pow(self.__cur_pos[0][0] - self.__start_pos[0][0], 2) +
math.pow(self.__cur_pos[0][1] - self.__start_pos[0][1], 2))
def __get_start_position(self):
ret = False
try:
self.__start_pos = self.__listen.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
self.__start_yaw = tf.transformations.euler_from_quaternion(
self.__start_pos[1])[2]
ret = True
except (tf.LookupException, tf.ConnectivityException), err:
rospy.loginfo("could not locate vehicle")
return ret
class PR2Arm():
wipe_started = False
standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips)
torso_min = 0.001 #115
torso_max = 0.299 #0.295
dist = 0.
move_arm_error_dict = {
-1: "PLANNING_FAILED: Could not plan a clear path to goal.",
0: "Succeeded [0]",
1: "Succeeded [1]",
-2: "TIMED_OUT",
-3: "START_STATE_IN_COLLISION: Try freeing the arms manually.",
-4: "START_STATE_VIOLATES_PATH_CONSTRAINTS",
-5: "GOAL_IN_COLLISION",
-6: "GOAL_VIOLATES_PATH_CONSTRAINTS",
-7: "INVALID_ROBOT_STATE",
-8: "INCOMPLETE_ROBOT_STATE",
-9: "INVALID_PLANNER_ID",
-10: "INVALID_NUM_PLANNING_ATTEMPTS",
-11: "INVALID_ALLOWED_PLANNING_TIME",
-12: "INVALID_GROUP_NAME",
-13: "INVALID_GOAL_JOINT_CONSTRAINTS",
-14: "INVALID_GOAL_POSITION_CONSTRAINTS",
-15: "INVALID_GOAL_ORIENTATION_CONSTRAINTS",
-16: "INVALID_PATH_JOINT_CONSTRAINTS",
-17: "INVALID_PATH_POSITION_CONSTRAINTS",
-18: "INVALID_PATH_ORIENTATION_CONSTRAINTS",
-19: "INVALID_TRAJECTORY",
-20: "INVALID_INDEX",
-21: "JOINT_LIMITS_VIOLATED",
-22: "PATH_CONSTRAINTS_VIOLATED",
-23: "COLLISION_CONSTRAINTS_VIOLATED",
-24: "GOAL_CONSTRAINTS_VIOLATED",
-25: "JOINTS_NOT_MOVING",
-26: "TRAJECTORY_CONTROLLER_FAILED",
-27: "FRAME_TRANSFORM_FAILURE",
-28: "COLLISION_CHECKING_UNAVAILABLE",
-29: "ROBOT_STATE_STALE",
-30: "SENSOR_INFO_STALE",
-31: "NO_IK_SOLUTION: Cannot reach goal configuration.",
-32: "INVALID_LINK_NAME",
-33: "IK_LINK_IN_COLLISION: Cannot reach goal configuration\
without contact.",
-34: "NO_FK_SOLUTION",
-35: "KINEMATICS_STATE_IN_COLLISION",
-36: "INVALID_TIMEOUT"
}
def __init__(self, arm, tfListener=None):
self.arm = arm
if not (self.arm == "right" or self.arm == "left"):
rospy.logerr("Failed to initialize PR2Arm: \
Must pass 'right' or 'left'")
return
if tfListener is None:
self.tf = TransformListener()
else:
self.tf = tfListener
if self.arm == "right":
# PR2: self.arm[0]+ 'arm_controller/joint_trajectory_action',
arm_controller = "/arm_controllerR/follow_joint_trajectory"
else:
arm_controller = "/arm_controller/follow_joint_trajectory"
self.arm_traj_client = actionlib.SimpleActionClient(
arm_controller, FollowJointTrajectoryAction)
rospy.loginfo("Waiting for " + arm_controller)
if self.arm_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found " + arm_controller)
else:
rospy.logwarn("Cannot find " + arm_controller)
# same for both pr2 and pr2lite
self.torso_client = actionlib.SimpleActionClient(
'torso_controller/position_joint_action',
SingleJointPositionAction)
rospy.loginfo("Waiting for torso_controller/position_joint_action \
server")
if self.torso_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo(
"Found torso_controller/position_joint_action server")
else:
rospy.logwarn("Cannot find torso_controller/position_joint_action \
server")
self.gripper_client = actionlib.SimpleActionClient(
self.arm[0] + '_gripper_controller/gripper_action',
Pr2GripperCommandAction)
rospy.loginfo("Waiting for " + self.arm[0] +
"_gripper_controller/gripper_action server")
if self.gripper_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found " + self.arm[0] +
"_gripper_controller/gripper_action server")
else:
rospy.logwarn("Cannot find " + self.arm[0] +
"_gripper_controller/gripper_action server")
# rospy.Subscriber('torso_controller/state',
# JointTrajectoryControllerState, self.update_torso_state)
self.log_out = rospy.Publisher(rospy.get_name() + '/log_out', String)
rospy.loginfo("Waiting for " + self.arm.capitalize() +
" FK/IK Solver services")
try:
rospy.wait_for_service("/pr2lite_" + self.arm +
"_arm_kinematics/get_fk")
rospy.wait_for_service("/pr2lite_" + self.arm +
"_arm_kinematics/get_fk_solver_info")
rospy.wait_for_service("/pr2lite_" + self.arm +
"_arm_kinematics/get_ik")
rospy.wait_for_service("/pr2lite_" + self.arm +
"_arm_kinematics/get_constraint_aware_ik")
rospy.wait_for_service("/pr2lite_" + self.arm +
"_arm_kinematics/get_ik_solver_info")
self.fk_info_proxy = rospy.ServiceProxy(
"/pr2lite_" + self.arm + "_arm_kinematics/get_fk_solver_info",
GetKinematicSolverInfo)
self.fk_info = self.fk_info_proxy()
self.fk_pose_proxy = rospy.ServiceProxy(
"/pr2lite_" + self.arm + "_arm_kinematics/get_fk",
GetPositionFK, True)
self.ik_info_proxy = rospy.ServiceProxy(
"/pr2lite_" + self.arm + "_arm_kinematics/get_ik_solver_info",
GetKinematicSolverInfo)
self.ik_info = self.ik_info_proxy()
self.ik_constraint_aware_pose_proxy = rospy.ServiceProxy(
"/pr2lite_" + self.arm +
"_arm_kinematics/get_constraint_aware_ik",
GetConstraintAwarePositionIK, True)
self.ik_pose_proxy = rospy.ServiceProxy(
"/pr2lite_" + self.arm + "_arm_kinematics/get_ik",
GetPositionIK, True)
rospy.loginfo("Found FK/IK Solver services")
except:
rospy.logerr("Could not find FK/IK Solver services")
self.set_planning_scene_diff_name= \
'/environment_server/set_planning_scene_diff'
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
self.set_planning_scene_diff = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.set_planning_scene_diff(SetPlanningSceneDiffRequest())
rospy.Subscriber('joint_states', JointState, self.update_joint_state)
self.test_pose = rospy.Publisher("test_pose", PoseStamped)
def update_joint_state(self, jtcs):
self.joint_state_time = jtcs.header.stamp
# ARD
if len(self.joint_state_pos) == 0 and len(
self.ik_info.kinematic_solver_info.joint_names) > 0:
for joint_state_name in self.ik_info.kinematic_solver_info.joint_names:
self.joint_state_pos.append(0)
self.joint_state_vel.append(0)
i = 0
for joint in jtcs.name:
j = 0
for joint_state_name in self.ik_info.kinematic_solver_info.joint_names:
if joint == joint_state_name:
self.joint_state_pos[j] = jtcs.position[i]
self.joint_state_vel[j] = jtcs.velocity[i]
j += 1
if joint == "torso_lift_joint":
self.torso_state = jtcs.position[i]
i += 1
def curr_pose(self):
# (trans,rot) = self.tf.lookupTransform("/torso_lift_link",
# (trans,rot) = self.tf.lookupTransform("chess_board_raw",
# # ARD "/"+
# self.arm+"_wrist_roll_link",
# rospy.Time(0))
# print "chess_board_raw curr_pose "
# print Point(*trans)
# ARD
now = rospy.Time.now()
self.tf.waitForTransform("base_link", self.arm + "_wrist_roll_link",
now, rospy.Duration(1.0))
# (trans,rot) = self.tf.lookupTransform("/torso_lift_link",
(trans, rot) = self.tf.lookupTransform(
"base_link",
# ARD "/"+
self.arm + "_wrist_roll_link",
now)
cp = PoseStamped()
cp.header.stamp = rospy.Time.now()
# cp.header.frame_id = '/torso_lift_link'
cp.header.frame_id = 'base_link'
cp.pose.position = Point(*trans)
cp.pose.orientation = Quaternion(*rot)
return cp
def wait_for_stop(self, wait_time=1, timeout=60):
rospy.sleep(wait_time)
end_time = rospy.Time.now() + rospy.Duration(timeout)
while not self.is_stopped():
if rospy.Time.now() < end_time:
rospy.sleep(wait_time)
else:
raise
def is_stopped(self):
time = self.joint_state_time
vels = self.joint_state_vel
if (np.allclose(vels, np.zeros(7))
and (rospy.Time.now() - time) < rospy.Duration(0.1)):
return True
else:
return False
def adjust_elbow(self, f32):
request = self.form_ik_request(self.curr_pose())
angs = list(request.ik_request.ik_seed_state.joint_state.position)
angs[2] += f32.data
request.ik_request.ik_seed_state.joint_state.position = angs
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
self.send_joint_angles(ik_goal.solution.joint_state.position)
else:
rospy.loginfo("Cannot adjust elbow position further")
self.log_out.publish(data="Cannot adjust elbow position further")
def gripper(self, position, effort=30):
pos = np.clip(position, 0.0, 0.09)
goal = Pr2GripperCommandGoal()
goal.command.position = pos
goal.command.max_effort = effort
self.gripper_client.send_goal(goal)
finished_within_time = self.gripper_client.wait_for_result(
rospy.Duration(15))
if not (finished_within_time):
self.gripper_client.cancel_goal()
rospy.loginfo("Timed out moving " + self.arm + " gripper")
return False
else:
state = self.gripper_client.get_state()
result = self.gripper_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Gripper Action Succeeded")
return True
else:
rospy.loginfo("Gripper Action Failed")
rospy.loginfo("Failure Result: %s" % result)
return False
def pose_relative_trans(self, pose, x=0., y=0., z=0.):
"""Return a pose translated relative to a given pose."""
ps = deepcopy(pose)
M_trans = tft.translation_matrix([x, y, z])
q_ps = [
ps.pose.orientation.x, ps.pose.orientation.y,
ps.pose.orientation.z, ps.pose.orientation.w
]
M_rot = tft.quaternion_matrix(q_ps)
trans = np.dot(M_rot, M_trans)
ps.pose.position.x += trans[0][-1]
ps.pose.position.y += trans[1][-1]
ps.pose.position.z += trans[2][-1]
#print ps
return ps
def pose_relative_rot(self, pose, r=0., p=0., y=0., degrees=True):
"""Return a pose rotated relative to a given pose."""
ps = deepcopy(pose)
if degrees:
r = math.radians(r)
p = math.radians(p)
y = math.radians(y)
des_rot_mat = tft.euler_matrix(r, p, y)
q_ps = [
ps.pose.orientation.x, ps.pose.orientation.y,
ps.pose.orientation.z, ps.pose.orientation.w
]
state_rot_mat = tft.quaternion_matrix(q_ps)
final_rot_mat = np.dot(state_rot_mat, des_rot_mat)
ps.pose.orientation = Quaternion(
*tft.quaternion_from_matrix(final_rot_mat))
return ps
def find_approach(self, pose, standoff=0., axis='x'):
"""Return a PoseStamped pointed down the z-axis of input pose."""
ps = deepcopy(pose)
if axis == 'x':
ps = self.pose_relative_rot(ps, p=90)
ps = self.pose_relative_trans(ps, -standoff)
return ps
def calc_dist(self, ps1, ps2):
""" Return the cartesian distance between the points of 2 poses."""
p1 = ps1.pose.position
p2 = ps2.pose.position
return math.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2 +
(p1.z - p2.z)**2)
def hand_frame_move(self, x, y=0, z=0, duration=None):
cp = self.curr_pose()
newpose = self.pose_relative_trans(cp, x, y, z)
self.dist = self.calc_dist(cp, newpose)
return newpose
def build_trajectory(self,
finish,
start=None,
ik_space=0.10,
duration=None,
tot_points=None):
if start == None: # if given one pose, use current position as start, else, assume (start, finish)
start = self.curr_pose()
print "start fro curr_pos"
print start
#TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK
# Based upon distance to travel, determine the number of intermediate steps required
# find linear increments of position.
print "finish pose"
print finish
dist = self.calc_dist(start, finish) #Total distance to travel
ik_steps = int(round(dist / ik_space) + 1.)
rospy.loginfo("IK Steps: %s Distance: %s " % (ik_steps, dist))
if tot_points is None:
tot_points = 1500 * dist
if duration is None:
duration = dist * 120
ik_fracs = np.linspace(
0, 1, ik_steps
) #A list of fractional positions along course to evaluate ik
tot_points = ik_steps # ARD
ang_fracs = np.linspace(0, 1, tot_points)
seed = None
x_gap = finish.pose.position.x - start.pose.position.x
y_gap = finish.pose.position.y - start.pose.position.y
z_gap = finish.pose.position.z - start.pose.position.z
print "Arm gaps: x ", x_gap, " y ", y_gap, " z ", z_gap
qs = [
start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w
]
qf = [
finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w
]
#For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
steps = [PoseStamped() for i in range(len(ik_fracs))]
for i, frac in enumerate(ik_fracs):
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_gap * frac
steps[i].pose.position.y = start.pose.position.y + y_gap * frac
# steps[i].pose.position.z = start.pose.position.z + z_gap*frac
# ARD: Chess hack for torso height. Really should fix time on tf?
steps[
i].pose.position.z = start.pose.position.z + z_gap * frac - .3
# steps[i].pose.position.z = start.pose.position.z + z_gap*frac
new_q = transformations.quaternion_slerp(qs, qf, frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
rospy.loginfo("Planning straight-line path, please wait")
self.log_out.publish(data="Planning straight-line path, please wait")
rospy.loginfo("frameid %s %s" %
(start.header.frame_id, finish.header.frame_id))
#Find initial ik for seeding
req = self.form_constraint_aware_ik_request(steps[0])
ik_goal = self.ik_constraint_aware_pose_proxy(req)
print "IK goal"
print req
# print req.pose_stamped.pose.position
# print req.ik_seed_state.name
# print req.ik_seed_state.position
for i, name in enumerate(ik_goal.solution.joint_state.name):
rospy.loginfo("traj joints %s" % name)
if name == 'left_upper_arm_hinge_joint' or name == 'right_upper_arm_hinge_joint':
hinge_index = i
elif name == 'left_shoulder_tilt_joint' or name == 'right_shoulder_tilt_joint':
shoulder_tilt_index = i
if len(ik_goal.solution.joint_state.position) == 0:
print "initial seeding failed"
seed = None
#ik_points = list([[0]*7 for i in range(len(ik_fracs))])
ik_points = list()
ik_fracs2 = list()
for i, p in enumerate(steps):
if (i == 0):
continue
#request = self.form_ik_request(p)
request = self.form_constraint_aware_ik_request(p)
if seed != None:
request.ik_request.ik_seed_state.joint_state.position = seed
request.ik_request.ik_seed_state.joint_state.name = ik_goal.solution.joint_state.name
ik_goal = self.ik_constraint_aware_pose_proxy(request)
print "IK goal %d" % (i)
print request
# print request.pose_stamped.pose.position
# print request.ik_seed_state.name
# print request.ik_seed_state.position
if len(ik_goal.solution.joint_state.position) != 0:
seed = list(ik_goal.solution.joint_state.position
) # seed the next ik w/previous points results
seed[hinge_index] = -1 * seed[shoulder_tilt_index]
ik_points.append(ik_goal.solution.joint_state.position)
ik_fracs2.append(ik_fracs[i])
print "IK solution for %d" % i
else:
print "No solution for %d" % i
rospy.loginfo("linear interp")
ik_fracs = ik_fracs2
if len(ik_points) == 0:
return None
ik_points = np.array(ik_points)
# Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i, :] = np.interp(ang_fracs, ik_fracs, ik_points[:,
i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
for name in traj.joint_names:
rospy.loginfo("traj joints %s" % name)
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:, i]
traj.points[i].velocities = [0] * 7
traj.points[i].time_from_start = rospy.Duration(ang_fracs[i] *
duration)
return traj
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def follow_trajectory(self, traj_goal):
self.arm_traj_client.send_goal(traj_goal)
self.arm_traj_client.wait_for_result(rospy.Duration(200))
def move_to_tolerance(self, pose):
attempt = 0
prevdist = 0
while True:
attempt += 1
print "Attempt %d" % attempt
traj = self.build_trajectory(pose, None)
if traj != None:
goal = self.build_follow_trajectory(traj)
self.follow_trajectory(goal)
curpos = self.curr_pose()
x_gap = pose.pose.position.x - curpos.pose.position.x
y_gap = pose.pose.position.y - curpos.pose.position.y
z_gap = pose.pose.position.z - curpos.pose.position.z
dist = self.calc_dist(curpos, pose) #Total distance to travel
print "Arm dist ", dist, " gaps: x ", x_gap, " y ", y_gap, " z ", z_gap
if abs(x_gap) <= pose.absolute_position_tolerance.x and abs(
y_gap) <= pose.absolute_position_tolerance.y and abs(
z_gap) <= pose.absolute_position_tolerance.z:
return
if abs(dist - prevdist) < .001: # .04 inches
return
prevdist = dist
def move_torso(self, pos):
rospy.loginfo("Moving Torso to reach arm goal")
goal_out = SingleJointPositionGoal()
goal_out.position = pos
self.torso_client.send_goal(goal_out)
return True
def fast_move(self, ps, time=0.):
ik_goal = self.ik_pose_proxy(self.form_ik_request(ps))
if ik_goal.error_code.val == 1:
point = JointTrajectoryPoint()
point.positions = ik_goal.solution.joint_state.position
self.send_traj_point(point)
else:
rospy.logerr("FAST MOVE IK FAILED!")
def blind_move(self, ps, duration=None):
(reachable, ik_goal, duration) = self.full_ik_check(ps)
if reachable:
self.send_joint_angles(ik_goal.solution.joint_state.position,
duration)
def check_torso(self, request):
"""
For unreachable goals, check to see if moving the torso can solve
the problem. If yes, return True, and torso adjustment needed.
If no, return False, and best possible Torso adjustment.
"""
goal_z = request.ik_request.pose_stamped.pose.position.z
torso_pos = self.torso_state.actual.positions[0]
spine_range = [self.torso_min - torso_pos, self.torso_max - torso_pos]
if abs(goal_z) < 0.005:
#Already at best position, dont try more (avoid moving to noise)
rospy.loginfo("Torso Already at best possible position")
return [False, 0]
#Find best possible case: shoulder @ goal height, max up, or max down.
if goal_z >= spine_range[0] and goal_z <= spine_range[1]:
rospy.loginfo("Goal within spine movement range")
request.ik_request.pose_stamped.pose.position.z = 0
opt_tor_move = goal_z
elif goal_z > spine_range[1]: #Goal is above possible shoulder height
rospy.loginfo("Goal above spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[1]
opt_tor_move = spine_range[1]
else: #Goal is below possible shoulder height
rospy.loginfo("Goal below spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[0]
opt_tor_move = spine_range[0]
#Check best possible case
print "Optimal Torso move: ", opt_tor_move
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val != 1:
#Return false: Not achievable, even for best case
rospy.loginfo("Original goal cannot be reached")
self.log_out.publish(data="Original goal cannot be reached")
return [False, opt_tor_move]
opt_ik_goal = ik_goal
#Achievable: Find a reasonable solution, move torso, return true
rospy.loginfo("Goal can be reached by moving spine")
self.log_out.publish(data="Goal can be reached by moving spine")
trials = np.linspace(0, opt_tor_move, round(abs(opt_tor_move) / 0.05))
np.append(trials, opt_tor_move)
rospy.loginfo("Torso far from best position, finding closer sol.")
print "Trials: ", trials
for trial in trials:
request.ik_request.pose_stamped.pose.position.z = goal_z + trial
print "Trying goal @ ", goal_z + trial
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
rospy.loginfo("Tried: %s, Succeeded" % trial)
return [True, trial]
rospy.loginfo("Tried: %s, Failed" % trial)
#Broke through somehow, catch error
return [True, opt_tor_move]
def full_ik_check(self, ps):
#Check goal as given, if reachable, return true
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
self.dist = self.calc_dist(self.curr_pose(), ps)
return (True, ik_goal, None)
#Check goal with vertical torso movement, if works, return true
(torso_succeeded, torso_move) = self.check_torso(req)
self.move_torso(self.torso_state.actual.positions[0] + torso_move)
duration = max(4, 85 * abs(torso_move))
if torso_succeeded:
ik_goal = self.ik_pose_proxy(req)
self.dist = self.calc_dist(self.curr_pose(), ps)
if ik_goal.error_code.val == 1:
return (True, ik_goal, duration)
else:
print "Reported Succeeded, then really failed: \r\n", ik_goal
#Check goal incrementally retreating hand pose, if works, return true
pct = 0.98
curr_pos = self.curr_pose().pose.position
dx = req.ik_request.pose_stamped.pose.position.x - curr_pos.x
dy = req.ik_request.pose_stamped.pose.position.y - curr_pos.y
while pct > 0.01:
#print "Percent: %s" %pct
req.ik_request.pose_stamped.pose.position.x = curr_pos.x + pct * dx
req.ik_request.pose_stamped.pose.position.y = curr_pos.y + pct * dy
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
rospy.loginfo("Succeeded PARTIALLY (%s) with torso" % pct)
return (True, ik_goal, duration)
else:
pct -= 0.02
#Nothing worked, report failure, return false
rospy.loginfo("IK Failed: Error Code %s" % str(ik_goal.error_code))
rospy.loginfo("Reached as far as possible")
self.log_out.publish(data="Cannot reach farther in this direction.")
return (False, ik_goal, duration)
def form_constraint_aware_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetConstraintAwarePositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = \
self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = \
self.ik_info.kinematic_solver_info.joint_names
req.ik_request.ik_seed_state.joint_state.position = \
self.joint_state_pos
return req
def form_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = \
self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = \
self.ik_info.kinematic_solver_info.joint_names
req.ik_request.ik_seed_state.joint_state.position = \
self.joint_state_pos
return req
def send_joint_angles(self, angles, duration=None):
point = JointTrajectoryPoint()
point.positions = angles
self.send_traj_point(point, duration)
def send_traj_point(self, point, time=None):
if time is None:
point.time_from_start = rospy.Duration(max(20 * self.dist, 4))
else:
point.time_from_start = rospy.Duration(time)
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
# ARD
# joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.header.frame_id = 'base_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0, 0, 0, 0, 0, 0, 0]
arm_goal = JointTrajectoryGoal()
arm_goal.trajectory = joint_traj
self.arm_traj_client.send_goal(arm_goal)
class ur5_grasp_project(object):
def __init__(self, joint_values=None):
rospy.init_node('command_GGCNN_ur5')
self.joint_values_home = joint_values
self.tf = TransformListener()
# Used to change the controller
self.controller_switch = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
# actionClient used to send joint positions
self.client = actionlib.SimpleActionClient(
'pos_based_pos_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
print("Waiting for server (pos_based_pos_traj_controller)...")
self.client.wait_for_server()
print("Connected to server (pos_based_pos_traj_controller)")
self.picking = False # Tells the node that the object must follow the gripper
##################
# Gazebo Related #
##################
if args.gazebo:
self.get_model_coordinates = rospy.ServiceProxy(
'/gazebo/get_link_state', GetLinkState)
self.grasp_started = rospy.Publisher(
'grasp_started', Bool, queue_size=1) # Detection flag
self.grasp_object_name = rospy.Publisher('grasp_object_name',
String,
queue_size=1)
self.grasp_object_position = rospy.Publisher(
'grasp_object_position', Pose, queue_size=1)
# Subscriber used to read joint values
rospy.Subscriber('/joint_states',
JointState,
self.ur5_actual_position_callback,
queue_size=1)
rospy.sleep(1.0)
# USED FOR COLLISION DETECTION
self.finger_links = [
'robotiq_85_right_finger_tip_link',
'robotiq_85_left_finger_tip_link'
]
# LEFT GRIPPER
self.string = ""
rospy.Subscriber(
'/left_finger_bumper_vals', ContactsState,
self.monitor_contacts_left_finger_callback) # ContactState
self.left_collision = False
self.contactState_left = ContactState()
# RIGHT GRIPPER
rospy.Subscriber(
'/right_finger_bumper_vals', ContactsState,
self.monitor_contacts_right_finger_callback) # ContactState
self.right_collision = False
self.contactState_right = ContactState()
self.client_gripper = actionlib.SimpleActionClient(
'gripper_controller_pos/follow_joint_trajectory',
FollowJointTrajectoryAction)
print("Waiting for server (gripper_controller_pos)...")
self.client_gripper.wait_for_server()
print("Connected to server (gripper_controller_pos)")
#################
# GGCNN Related #
#################
self.posCB = []
self.ori = []
self.grasp_cartesian_pose = []
self.gripper_angle_grasp = 0.0
self.final_orientation = 0.0
if args.gazebo:
self.GGCNN_offset_x = 0.0
self.GGCNN_offset_y = 0.0
self.GGCNN_offset_z = 0.020 # 0.019
else:
self.GGCNN_offset_x = -0.03 # 0.002
self.GGCNN_offset_y = 0.02 # -0.05
self.GGCNN_offset_z = 0.058 # 0.013
self.ur5_joint_names = rospy.get_param("/ur5_joint_names")
self.robotiq_joint_name = rospy.get_param("/robotiq_joint_name")
# Topic published from GG-CNN Node
rospy.Subscriber('ggcnn/out/command',
Float32MultiArray,
self.ggcnn_command_callback,
queue_size=1)
####################
# Pipeline Related #
####################
self.classes = rospy.get_param("/classes")
# Topics related to the new grasping pipeline
rospy.Subscriber('flags/grasp_ready',
Bool,
self.grasp_ready_callback,
queue_size=1) # Grasp flag
rospy.Subscriber('flags/reposition_robot_flag',
Bool,
self.reposition_robot_callback,
queue_size=1) # Reposition flag
rospy.Subscriber('flags/detection_ready',
Bool,
self.detection_ready_callback,
queue_size=1) # Detection flag
rospy.Subscriber('reposition_coord',
Float32MultiArray,
self.reposition_coord_callback,
queue_size=1)
rospy.Subscriber('/selective_grasping/tag_detections',
Int16MultiArray,
self.tags_callback,
queue_size=1)
self.tags = [
'tag_0_corrected', 'tag_1_corrected', 'tag_2_corrected',
'tag_3_corrected', 'tag_4_corrected', 'tag_5_corrected',
'tag_6_corrected', 'tag_7_corrected'
]
if args.gazebo:
self.tags_position_offset = [0.062, 0.0, 0.062]
self.grasp_flag = False
self.detected_tags = []
self.require_class = rospy.Publisher('pipeline/required_class',
Int8,
queue_size=1)
###################
# Robotiq Related #
###################
self.pub_gripper_command = rospy.Publisher(
'Robotiq2FGripperRobotOutput',
outputMsg.Robotiq2FGripper_robot_output,
queue_size=1)
self.d = None # msg received from GGCN
self.gripper_max_width = 0.14
def turn_velocity_controller_on(self):
self.controller_switch(['joint_group_vel_controller'],
['pos_based_pos_traj_controller'], 1)
def turn_position_controller_on(self):
self.controller_switch(['pos_based_pos_traj_controller'],
['joint_group_vel_controller'], 1)
def turn_gripper_velocity_controller_on(self):
self.controller_switch(['gripper_controller_vel'],
['gripper_controller_pos'], 1)
def turn_gripper_position_controller_on(self):
self.controller_switch(['gripper_controller_pos'],
['gripper_controller_vel'], 1)
def tags_callback(self, msg):
self.detected_tags = msg.data
def grasp_ready_callback(self, msg):
self.grasp_flag = msg.data
def detection_ready_callback(self, msg):
self.detection_ready_flag = msg.data
def reposition_robot_callback(self, msg):
self.reposition_robot_flag = msg.data
def reposition_coord_callback(self, msg):
self.reposition_coords = msg.data
def monitor_contacts_left_finger_callback(self, msg):
if msg.states:
self.left_collision = True
string = msg.states[0].collision1_name
string_collision = re.findall(r'::(.+?)::', string)[0]
# print("Left String_collision: ", string_collision)
if string_collision in self.finger_links:
string = msg.states[0].collision2_name
# print("Left Real string (object): ", string)
self.string = re.findall(r'::(.+?)::', string)[0]
# print("Left before: ", self.string)
else:
self.string = string_collision
# print("Left in else: ", string_collision)
else:
self.left_collision = False
def monitor_contacts_right_finger_callback(self, msg):
if msg.states:
self.right_collision = True
string = msg.states[0].collision1_name
string_collision = re.findall(r'::(.+?)::', string)[0]
# print("Right String_collision: ", string_collision)
if string_collision in self.finger_links:
string = msg.states[0].collision2_name
# print("Right Real string (object): ", string)
self.string = re.findall(r'::(.+?)::', string)[0]
# print("Right before: ", self.string)
else:
self.string = string_collision
# print("Right in else: ", self.string)
else:
self.right_collision = False
def ur5_actual_position_callback(self, joint_values_from_ur5):
"""Get UR5 joint angles
The joint states published by /joint_staes of the UR5 robot are in wrong order.
/joint_states topic normally publishes the joint in the following order:
[elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
But the correct order of the joints that must be sent to the robot is:
['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Arguments:
joint_values_from_ur5 {list} -- Actual angles of the UR5 Robot
"""
if args.gazebo:
self.th3, self.robotic, self.th2, self.th1, self.th4, self.th5, self.th6 = joint_values_from_ur5.position
# print("Robotic angle: ", self.robotic)
else:
self.th3, self.th2, self.th1, self.th4, self.th5, self.th6 = joint_values_from_ur5.position
self.actual_position = [
self.th1, self.th2, self.th3, self.th4, self.th5, self.th6
]
def ggcnn_command_callback(self, msg):
"""
GGCNN Command Subscriber Callback
"""
self.tf.waitForTransform("base_link", "object_detected", rospy.Time(0),
rospy.Duration(4.0)) # rospy.Time.now()
object_pose, object_ori = self.tf.lookupTransform(
"base_link", "object_detected", rospy.Time(0))
self.d = list(msg.data)
object_pose[0] += self.GGCNN_offset_x
object_pose[1] += self.GGCNN_offset_y
object_pose[2] += self.GGCNN_offset_z
self.posCB = object_pose
self.ori = self.d[3]
br = TransformBroadcaster()
br.sendTransform((object_pose[0], object_pose[1], object_pose[2]),
quaternion_from_euler(0.0, 0.0, self.ori),
rospy.Time.now(), "object_link", "base_link")
def get_link_position_picking(self):
link_name = self.string
model_coordinates = self.get_model_coordinates(self.string,
'wrist_3_link')
self.model_pose_picking = model_coordinates.link_state.pose
self.grasp_object_position.publish(
Pose(self.model_pose_picking.position,
self.model_pose_picking.orientation))
def get_ik(self, position):
"""Get the inverse kinematics
Get the inverse kinematics of the UR5 robot using track_IK package giving a desired intial position
Arguments:
position {list} -- A position representing x, y and z
Returns:
sol {list} -- Joint angles or None if track_ik is not able to find a valid solution
"""
camera_support_angle_offset = 0.0
q = quaternion_from_euler(0.0, -3.14 + camera_support_angle_offset,
0.0)
# Joint order:
# ('shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'grasping_link')
ik_solver = IK("base_link", "grasping_link", solve_type="Distance")
sol = ik_solver.get_ik([
0.2201039360819781, -1.573845095552878, -1.521853400505349,
-1.6151347051274518, 1.5704492904506875, 0.0
], position[0], position[1], position[2], q[0], q[1], q[2], q[3])
if sol is not None:
sol = list(sol)
sol[-1] = 0.0
else:
print("IK didn't return any solution")
return sol
def __build_goal_message_ur5(self):
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.joint_names = self.ur5_joint_names
goal.goal_tolerance.append(
JointTolerance('joint_tolerance', 0.1, 0.1, 0))
goal.goal_time_tolerance = rospy.Duration(5, 0)
return goal
def traj_planner(self,
cart_pos,
grasp_step='move',
way_points_number=10,
movement='slow'):
"""Quintic Trajectory Planner
Publish a trajectory to UR5 using quintic splines.
Arguments:
cart_pos {[float]} -- Grasp position [x, y, z]
Keyword Arguments:
grasp_step {str} -- Set UR5 movement type (default: {'move'})
way_points_number {number} -- Number of points considered in trajectory (default: {10})
movement {str} -- Movement speed (default: {'slow'})
"""
offset_from_object = 0.05
if grasp_step == 'pregrasp':
self.grasp_cartesian_pose = deepcopy(self.posCB)
self.grasp_cartesian_pose[-1] += offset_from_object
joint_pos = self.get_ik(self.grasp_cartesian_pose)
if joint_pos is not None:
joint_pos[-1] = self.ori
self.final_orientation = deepcopy(self.ori)
self.gripper_angle_grasp = deepcopy(self.d[-2])
elif grasp_step == 'grasp':
self.grasp_cartesian_pose[-1] -= offset_from_object
joint_pos = self.get_ik(self.grasp_cartesian_pose)
if joint_pos is not None:
joint_pos[-1] = self.final_orientation
elif grasp_step == 'move':
joint_pos = self.get_ik(cart_pos)
if joint_pos is not None:
joint_pos[-1] = 0.0
if joint_pos is not None:
if movement == 'slow':
final_traj_duration = 500.0 # total iteractions
elif movement == 'fast':
final_traj_duration = 350.0
v0 = a0 = vf = af = 0
t0 = 5.0
tf = (t0 +
final_traj_duration) / way_points_number # tf by way point
t = tf / 10 # for each movement
ta = tf / 10 # to complete each movement
a = [0.0] * 6
pos_points, vel_points, acc_points = [0.0] * 6, [0.0] * 6, [0.0
] * 6
goal = self.__build_goal_message_ur5()
for i in range(6):
q0 = self.actual_position[i]
qf = joint_pos[i]
b = np.array([q0, v0, a0, qf, vf, af]).transpose()
m = np.array([[1, t0, t0**2, t0**3, t0**4, t0**5],
[0, 1, 2 * t0, 3 * t0**2, 4 * t0**3, 5 * t0**4],
[0, 0, 2, 6 * t0, 12 * t0**2, 20 * t0**3],
[1, tf, tf**2, tf**3, tf**4, tf**5],
[0, 1, 2 * tf, 3 * tf**2, 4 * tf**3, 5 * tf**4],
[0, 0, 2, 6 * tf, 12 * tf**2, 20 * tf**3]])
a[i] = np.linalg.inv(m).dot(b)
for i in range(way_points_number):
for j in range(6):
pos_points[j] = a[j][0] + a[j][1] * t + a[j][2] * t**2 + a[
j][3] * t**3 + a[j][4] * t**4 + a[j][5] * t**5
vel_points[j] = a[j][1] + 2 * a[j][2] * t + 3 * a[j][
3] * t**2 + 4 * a[j][4] * t**3 + 5 * a[j][5] * t**4
acc_points[j] = 2 * a[j][2] + 6 * a[j][3] * t + 12 * a[j][
4] * t**2 + 20 * a[j][5] * t**3
goal.trajectory.points.append(
JointTrajectoryPoint(
positions=pos_points,
velocities=vel_points,
accelerations=acc_points,
time_from_start=rospy.Duration(t))) #default 0.1*i + 5
t += ta
self.client.send_goal(goal)
self.all_close(joint_pos)
else:
print('Could not find a IK solution')
def all_close(self, goal, tolerance=0.00005):
"""Wait until goal is reached in configuration space
This method check if the robot reached goal position since wait_for_result seems to be broken
Arguments:
goal {[list]} -- Goal in configuration space (joint values)
Keyword Arguments:
tolerance {number} -- Minimum error allowed to consider the trajectory completed (default: {0.00005})
"""
error = np.sum([(self.actual_position[i] - goal[i])**2
for i in range(6)])
print("> Waiting for the trajectory to finish...")
while not rospy.is_shutdown() and error > tolerance:
error = np.sum([(self.actual_position[i] - goal[i])**2
for i in range(6)])
if error < tolerance:
print(
"> Trajectory Suceeded!\n") # whithin the tolerance specified
else:
rospy.logerr("> Trajectory aborted.")
def genCommand(self, char, command, pos=None):
"""
Update the command according to the character entered by the user.
"""
if char == 'a':
# command = outputMsg.Robotiq2FGripper_robot_output();
command.rACT = 1 # Gripper activation
command.rGTO = 1 # Go to position request
command.rSP = 255 # Speed
command.rFR = 150 # Force
if char == 'r':
command.rACT = 0
if char == 'c':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = 255
command.rSP = 40
command.rFR = 150
# @param pos Gripper width in meters. [0, 0.087]
if char == 'p':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = int(
np.clip(
(13. - 230.) / self.gripper_max_width * self.ori + 230., 0,
255))
command.rSP = 40
command.rFR = 150
if char == 'o':
command.rACT = 1
command.rGTO = 1
command.rATR = 0
command.rPR = 0
command.rSP = 40
command.rFR = 150
return command
def command_gripper(self, action):
command = outputMsg.Robotiq2FGripper_robot_output()
command = self.genCommand(action, command)
self.pub_gripper_command.publish(command)
def gripper_send_position_goal(self,
position=0.3,
velocity=0.4,
action='move'):
"""Send position goal to the gripper
Keyword Arguments:
position {float} -- Gripper angle (default: {0.3})
velocity {float} -- Gripper velocity profile (default: {0.4})
action {str} -- Gripper movement (default: {'move'})
"""
self.turn_gripper_position_controller_on()
duration = 0.2
if action == 'pre_grasp_angle':
max_distance = 0.085
angular_coeff = 0.11
K = 1 # lower numbers = higher angles
angle = (max_distance -
self.gripper_angle_grasp) / angular_coeff * K
position = angle
velocity = 0.4
elif action == 'pick':
position = 0.7
velocity = 0.05
duration = 8.0
goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()
goal.trajectory.joint_names = self.robotiq_joint_name
goal.trajectory.points.append(
JointTrajectoryPoint(positions=[position],
velocities=[velocity],
accelerations=[0.0],
time_from_start=rospy.Duration(duration)))
self.client_gripper.send_goal(goal)
if action == 'pick':
while not rospy.is_shutdown(
) and not self.left_collision and not self.right_collision:
pass
self.client_gripper.cancel_goal()
def move_home_on_shutdown(self):
self.client.cancel_goal()
self.client_gripper.cancel_goal()
print("Shutting down node...")
def grasp_main(self, point_init_home, depth_shot_point):
random_classes = [i for i in range(len(self.classes))]
bin_location = [[-0.65, -0.1, 0.2], [-0.65, 0.1, 0.2]]
garbage_location = [-0.4, -0.30, 0.10]
raw_input('=== Press enter to start the grasping process')
while not rospy.is_shutdown():
# The grasp is performed randomly and the chosen object is published to
# the detection node
if not len(random_classes):
print(
"\n> IMPORTANT! There is no object remaining in the object's picking list. Resetting the objects list...\n"
)
random_classes = [i for i in range(len(self.classes))]
random_object_id = random.sample(random_classes, 1)[0]
random_classes.pop(random_classes.index(random_object_id))
print('> Object to pick: ', self.classes[random_object_id])
self.require_class.publish(random_object_id)
# Before pressing ENTER you should observe if the GG-CNN is getting the
# right grasp on the selected object (green point in the grasp image)
# just for safety
raw_input("==== Press enter to start the grasping process!")
if self.detection_ready_flag:
if self.reposition_robot_flag:
print('repos flag: ', self.reposition_robot_flag)
print('> Repositioning robot...')
self.tf.waitForTransform("base_link", "grasping_link",
rospy.Time.now(),
rospy.Duration(4.0))
eef_pose, _ = self.tf.lookupTransform(
"base_link", "grasping_link", rospy.Time(0))
corrected_position = [
eef_pose[0] - self.reposition_coords[1],
eef_pose[1] + self.reposition_coords[0], eef_pose[2]
]
self.traj_planner(corrected_position, movement='fast')
raw_input("==== Press enter when the trajectory finishes!")
if self.grasp_flag and self.detection_ready_flag and not self.reposition_robot_flag:
print('> Moving to the grasping position... \n')
# print("Grasp flag: " + str(self.grasp_flag))
# print("Reposition robot flag: " + str( self.reposition_robot_flag))
self.traj_planner([], 'pregrasp', movement='fast')
# It closes the gripper before approaching the object
# It prevents the gripper to collide with other objects when grasping
if args.gazebo:
self.gripper_send_position_goal(
action='pre_grasp_angle')
else:
self.command_gripper('p')
# Moving the robot to pick the object - BE CAREFULL!
self.traj_planner([], 'grasp', movement='slow')
print("Picking object...")
if args.gazebo:
self.gripper_send_position_goal(action='pick')
self.get_link_position_picking()
self.grasp_started.publish(True)
self.grasp_object_name.publish(self.string)
else:
raw_input("==== Press enter to close the gripper!")
self.command_gripper('c')
# After a collision is detected, the arm will start the picking action
raw_input("==== Press enter to move the object to the bin")
if random_object_id < 5:
self.traj_planner(bin_location[0], movement='fast')
else:
self.traj_planner(bin_location[1], movement='fast')
rospy.sleep(2.0) # waiting for the tag detections
number_of_detected_tags = sum(self.detected_tags)
if number_of_detected_tags:
selected_tag_status = self.detected_tags[
random_object_id] # if the tag was detected or not
print('selected tag: ', self.tags[random_object_id])
if selected_tag_status:
self.tf.waitForTransform(
"base_link", self.tags[random_object_id],
rospy.Time(0),
rospy.Duration(2.0)) # rospy.Time.now()
ptFinal, oriFinal = self.tf.lookupTransform(
"base_link", self.tags[random_object_id],
rospy.Time(0))
ptFinal = [
ptFinal[i] + self.tags_position_offset[i]
for i in range(3)
]
raw_input("==== go to tag")
pt_inter = deepcopy(ptFinal)
pt_inter[-1] += 0.1
self.traj_planner(pt_inter, movement='fast')
self.traj_planner(ptFinal, movement='fast')
else:
# In this case, the camera identifies some other tag but not the tag corresponding
# to the object in case
print(
'> Could not find the box tag. Placing the object anywhere \n'
)
raw_input('=== Pres enter to proceed')
self.traj_planner(garbage_location,
movement='fast')
else:
# In this case, the robot cannot find any tag
print(
'> Could not find any tag. Placing the object anywhere \n'
)
raw_input('=== Pres enter to proceed')
self.traj_planner(garbage_location, movement='fast')
print("Placing object...")
# After the bin location is reached, the robot will place the object and move back
# to the initial position
# self.picking = False # Detach object
if args.gazebo:
self.gripper_send_position_goal(0.3)
# self.delete_model_service_method() # Not working anymore - need to investigate
self.grasp_started.publish(False)
else:
self.command_gripper('o')
print("> Moving back to home position...\n")
self.traj_planner(point_init_home, movement='fast')
self.traj_planner(depth_shot_point, movement='slow')
print('> Grasping finished \n')
else:
print('> The requested object could not be grasped! \n')
self.traj_planner(depth_shot_point, movement='fast')
else:
print('> The requested object was not detected! \n')
class NavigationManager(object):
def __init__(self): #Initializes serveces and clients
rospy.init_node("NavManager_node", anonymous=True)
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_base_client.wait_for_server(rospy.Duration(5))
rospy.Service('move_to_goal', NavigationGoal, self.goal_handler)
rospy.Service('reposition', NavigationRepose, self.repose_handler)
self.reset_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty)
self.tf_listener = TransformListener()
self.goal = MoveBaseGoal()
#rospy.Timer(rospy.Duration(secs=3,nsecs=0), self.clear_costmap)
rospy.spin()
def goal_handler(self, data): #Processes the NavigationGoal service
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.x = data.pose.position.x
self.goal.target_pose.pose.position.y = data.pose.position.y
self.goal.target_pose.pose.orientation.z = data.pose.orientation.z
self.goal.target_pose.pose.orientation.w = data.pose.orientation.w
status = self.send_goal()
return status
def repose_handler(self, data): #Processes the NavigationRepose service
x_req = data.target.x
y_req = data.target.y
linear, angular = self.get_repose_goal_coords(x_req, y_req)
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.x = linear[0]
self.goal.target_pose.pose.position.y = linear[1]
self.goal.target_pose.pose.orientation.z = angular[2]
self.goal.target_pose.pose.orientation.w = angular[3]
status = self.send_goal()
return status
def send_goal(self): #Sends a goal to move_base using actionlib
self.clear_costmap()
time.sleep(0.2)
self.move_base_client.send_goal(self.goal)
status = self.move_base_client.wait_for_result()
if not status:
self.move_base_client.cancel_goal()
rospy.loginfo("move_base failed")
return 1 #False
else:
state = self.move_base_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal reached")
return 0 #True
rospy.loginfo("Can't reach goal")
return 1 #False
def get_repose_goal_coords(
self, x_req, y_req
): #Uses the robots actual postion to calcualte the new reposition goal
(linear,
angular) = self.tf_listener.lookupTransform('base_link', 'map',
rospy.Time(0))
current_x = linear[0]
current_y = linear[1]
current_theta = euler_from_quaternion(angular)
new_x = ((cos(current_theta[2]) * x_req) -
(sin(current_theta[2]) * y_req))
new_y = ((cos(current_theta[2]) * y_req) +
(sin(current_theta[2]) * x_req))
linear[0] += new_x
linear[1] += new_y
return linear, angular
def clear_costmap(self): #Calls the clear costmap service
self.reset_costmap()
rospy.loginfo('Costmap cleared')
class PR2Arm():
wipe_started = False
standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips)
torso_min = 0.001 #115
torso_max = 0.299 #0.295
dist = 0.
move_arm_error_dict = {
-1 : "PLANNING_FAILED: Could not plan a clear path to goal.",
0 : "Succeeded [0]",
1 : "Succeeded [1]",
-2 : "TIMED_OUT",
-3 : "START_STATE_IN_COLLISION: Try freeing the arms manually.",
-4 : "START_STATE_VIOLATES_PATH_CONSTRAINTS",
-5 : "GOAL_IN_COLLISION",
-6 : "GOAL_VIOLATES_PATH_CONSTRAINTS",
-7 : "INVALID_ROBOT_STATE",
-8 : "INCOMPLETE_ROBOT_STATE",
-9 : "INVALID_PLANNER_ID",
-10 : "INVALID_NUM_PLANNING_ATTEMPTS",
-11 : "INVALID_ALLOWED_PLANNING_TIME",
-12 : "INVALID_GROUP_NAME",
-13 : "INVALID_GOAL_JOINT_CONSTRAINTS",
-14 : "INVALID_GOAL_POSITION_CONSTRAINTS",
-15 : "INVALID_GOAL_ORIENTATION_CONSTRAINTS",
-16 : "INVALID_PATH_JOINT_CONSTRAINTS",
-17 : "INVALID_PATH_POSITION_CONSTRAINTS",
-18 : "INVALID_PATH_ORIENTATION_CONSTRAINTS",
-19 : "INVALID_TRAJECTORY",
-20 : "INVALID_INDEX",
-21 : "JOINT_LIMITS_VIOLATED",
-22 : "PATH_CONSTRAINTS_VIOLATED",
-23 : "COLLISION_CONSTRAINTS_VIOLATED",
-24 : "GOAL_CONSTRAINTS_VIOLATED",
-25 : "JOINTS_NOT_MOVING",
-26 : "TRAJECTORY_CONTROLLER_FAILED",
-27 : "FRAME_TRANSFORM_FAILURE",
-28 : "COLLISION_CHECKING_UNAVAILABLE",
-29 : "ROBOT_STATE_STALE",
-30 : "SENSOR_INFO_STALE",
-31 : "NO_IK_SOLUTION: Cannot reach goal configuration.",
-32 : "INVALID_LINK_NAME",
-33 : "IK_LINK_IN_COLLISION: Cannot reach goal configuration\
without contact.",
-34 : "NO_FK_SOLUTION",
-35 : "KINEMATICS_STATE_IN_COLLISION",
-36 : "INVALID_TIMEOUT"
}
def __init__(self, arm, tfListener=None):
self.arm = arm
if not (self.arm == "right" or self.arm=="left"):
rospy.logerr("Failed to initialize PR2Arm: \
Must pass 'right' or 'left'")
return
if tfListener is None:
self.tf = TransformListener()
else:
self.tf = tfListener
self.move_arm_client = actionlib.SimpleActionClient(
'move_' + self.arm + '_arm', MoveArmAction)
rospy.loginfo("Waiting for move_" + self.arm + "_arm server")
if self.move_arm_client.wait_for_server(rospy.Duration(50)):
rospy.loginfo("Found move_" + self.arm + "_arm server")
else:
rospy.logwarn("Cannot find move_" + self.arm + "_arm server")
self.arm_traj_client = actionlib.SimpleActionClient(
self.arm[0]+
'_arm_controller/joint_trajectory_action',
JointTrajectoryAction)
rospy.loginfo("Waiting for " + self.arm[0] +
"_arm_controller/joint_trajectory_action server")
if self.arm_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found "+self.arm[0]+
"_arm_controller/joint_trajectory_action server")
else:
rospy.logwarn("Cannot find " + self.arm[0] +
" _arm_controller/joint_trajectory_action server")
self.arm_follow_traj_client = actionlib.SimpleActionClient(self.arm[0]+
'_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
rospy.loginfo("Waiting for "+self.arm[0]+
"_arm_controller/follow_joint_trajectory server")
if self.arm_follow_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found "+self.arm[0]+
"_arm_controller/follow_joint_trajectory server")
else:
rospy.logwarn("Cannot find "+self.arm[0]+
"_arm_controller/follow_joint_trajectory server")
self.torso_client = actionlib.SimpleActionClient(
'torso_controller/position_joint_action',
SingleJointPositionAction)
rospy.loginfo("Waiting for torso_controller/position_joint_action \
server")
if self.torso_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found torso_controller/position_joint_action server")
else:
rospy.logwarn("Cannot find torso_controller/position_joint_action \
server")
self.gripper_client = actionlib.SimpleActionClient(
self.arm[0]+'_gripper_controller/gripper_action',
Pr2GripperCommandAction)
rospy.loginfo("Waiting for "+self.arm[0]+
"_gripper_controller/gripper_action server")
if self.gripper_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found "+self.arm[0]+
"_gripper_controller/gripper_action server")
else:
rospy.logwarn("Cannot find "+self.arm[0]+
"_gripper_controller/gripper_action server")
rospy.Subscriber(self.arm[0]+'_arm_controller/state',
JointTrajectoryControllerState, self.update_joint_state)
rospy.Subscriber('torso_controller/state',
JointTrajectoryControllerState, self.update_torso_state)
self.log_out = rospy.Publisher(rospy.get_name()+'/log_out', String)
rospy.loginfo("Waiting for "+self.arm.capitalize()+" FK/IK Solver services")
try:
rospy.wait_for_service("/pr2_"+self.arm+"_arm_kinematics/get_fk")
rospy.wait_for_service("/pr2_"+self.arm+
"_arm_kinematics/get_fk_solver_info")
rospy.wait_for_service("/pr2_"+self.arm+"_arm_kinematics/get_ik")
rospy.wait_for_service("/pr2_"+self.arm+
"_arm_kinematics/get_ik_solver_info")
self.fk_info_proxy = rospy.ServiceProxy(
"/pr2_"+self.arm+
"_arm_kinematics/get_fk_solver_info",
GetKinematicSolverInfo)
self.fk_info = self.fk_info_proxy()
self.fk_pose_proxy = rospy.ServiceProxy(
"/pr2_"+self.arm+"_arm_kinematics/get_fk",
GetPositionFK, True)
self.ik_info_proxy = rospy.ServiceProxy(
"/pr2_"+self.arm+
"_arm_kinematics/get_ik_solver_info",
GetKinematicSolverInfo)
self.ik_info = self.ik_info_proxy()
self.ik_pose_proxy = rospy.ServiceProxy(
"/pr2_"+self.arm+"_arm_kinematics/get_ik",
GetPositionIK, True)
rospy.loginfo("Found FK/IK Solver services")
except:
rospy.logerr("Could not find FK/IK Solver services")
self.set_planning_scene_diff_name= \
'/environment_server/set_planning_scene_diff'
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
self.set_planning_scene_diff = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.set_planning_scene_diff(SetPlanningSceneDiffRequest())
self.test_pose = rospy.Publisher("test_pose", PoseStamped)
def update_joint_state(self, jtcs):
self.joint_state_time = jtcs.header.stamp
self.joint_state_act = jtcs.actual
self.joint_state_des = jtcs.desired
def update_torso_state(self, jtcs):
self.torso_state = jtcs
def curr_pose(self):
(trans,rot) = self.tf.lookupTransform("/torso_lift_link",
"/"+self.arm[0]+"_wrist_roll_link",
rospy.Time(0))
cp = PoseStamped()
cp.header.stamp = rospy.Time.now()
cp.header.frame_id = '/torso_lift_link'
cp.pose.position = Point(*trans)
cp.pose.orientation = Quaternion(*rot)
return cp
def wait_for_stop(self, wait_time=1, timeout=60):
rospy.sleep(wait_time)
end_time = rospy.Time.now()+rospy.Duration(timeout)
while not self.is_stopped():
if rospy.Time.now()<end_time:
rospy.sleep(wait_time)
else:
raise
def is_stopped(self):
time = self.joint_state_time
vels = self.joint_state_act.velocities
if (np.allclose(vels, np.zeros(7)) and
(rospy.Time.now()-time)<rospy.Duration(0.1)):
return True
else:
return False
def adjust_elbow(self, f32):
request = self.form_ik_request(self.curr_pose())
angs = list(request.ik_request.ik_seed_state.joint_state.position)
angs[2] += f32.data
request.ik_request.ik_seed_state.joint_state.position = angs
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
self.send_joint_angles(ik_goal.solution.joint_state.position)
else:
rospy.loginfo("Cannot adjust elbow position further")
self.log_out.publish(data="Cannot adjust elbow position further")
def gripper(self, position, effort=30):
pos = np.clip(position,0.0, 0.09)
goal = Pr2GripperCommandGoal()
goal.command.position = pos
goal.command.max_effort = effort
self.gripper_client.send_goal(goal)
finished_within_time = self.gripper_client.wait_for_result(
rospy.Duration(15))
if not (finished_within_time):
self.gripper_client.cancel_goal()
rospy.loginfo("Timed out moving "+self.arm+" gripper")
return False
else:
state = self.gripper_client.get_state()
result = self.gripper_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Gripper Action Succeeded")
return True
else:
rospy.loginfo("Gripper Action Failed")
rospy.loginfo("Failure Result: %s" %result)
return False
def hand_frame_move(self, x, y=0, z=0, duration=None):
cp = self.curr_pose()
newpose = pu.pose_relative_trans(cp,x,y,z)
self.dist = pu.calc_dist(cp, newpose)
return newpose
def build_trajectory(self, finish, start=None, ik_space=0.005,
duration=None, tot_points=None):
if start == None: # if given one pose, use current position as start, else, assume (start, finish)
start = self.curr_pose()
#TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK
# Based upon distance to travel, determine the number of intermediate steps required
# find linear increments of position.
dist = pu.calc_dist(start, finish) #Total distance to travel
ik_steps = int(round(dist/ik_space)+1.)
print "IK Steps: %s" %ik_steps
if tot_points is None:
tot_points = 1500*dist
if duration is None:
duration = dist*120
ik_fracs = np.linspace(0, 1, ik_steps) #A list of fractional positions along course to evaluate ik
ang_fracs = np.linspace(0,1, tot_points)
x_gap = finish.pose.position.x - start.pose.position.x
y_gap = finish.pose.position.y - start.pose.position.y
z_gap = finish.pose.position.z - start.pose.position.z
qs = [start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w]
qf = [finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w]
#For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
steps = [PoseStamped() for i in range(len(ik_fracs))]
for i,frac in enumerate(ik_fracs):
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_gap*frac
steps[i].pose.position.y = start.pose.position.y + y_gap*frac
steps[i].pose.position.z = start.pose.position.z + z_gap*frac
new_q = transformations.quaternion_slerp(qs,qf,frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
rospy.loginfo("Planning straight-line path, please wait")
self.log_out.publish(data="Planning straight-line path, please wait")
#For each pose in trajectory, find ik angles
#Find initial ik for seeding
req = self.form_ik_request(steps[0])
ik_goal = self.ik_pose_proxy(req)
seed = ik_goal.solution.joint_state.position
ik_points = [[0]*7 for i in range(len(ik_fracs))]
for i, p in enumerate(steps):
request = self.form_ik_request(p)
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
ik_points[i] = ik_goal.solution.joint_state.position
seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
ik_points = np.array(ik_points)
# Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:,i]
traj.points[i].velocities = [0]*7
traj.points[i].time_from_start = rospy.Duration(
ang_fracs[i]*duration)
return traj
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def move_torso(self, pos):
rospy.loginfo("Moving Torso to reach arm goal")
goal_out = SingleJointPositionGoal()
goal_out.position = pos
self.torso_client.send_goal(goal_out)
return True
def fast_move(self, ps, time=0.):
ik_goal = self.ik_pose_proxy(self.form_ik_request(ps))
if ik_goal.error_code.val == 1:
point = JointTrajectoryPoint()
point.positions = ik_goal.solution.joint_state.position
self.send_traj_point(point)
else:
rospy.logerr("FAST MOVE IK FAILED!")
def blind_move(self, ps, duration = None):
(reachable, ik_goal, duration) = self.full_ik_check(ps)
if reachable:
self.send_joint_angles(ik_goal.solution.joint_state.position,
duration)
def check_torso(self, request):
"""
For unreachable goals, check to see if moving the torso can solve
the problem. If yes, return True, and torso adjustment needed.
If no, return False, and best possible Torso adjustment.
"""
goal_z = request.ik_request.pose_stamped.pose.position.z
torso_pos = self.torso_state.actual.positions[0]
spine_range = [self.torso_min - torso_pos, self.torso_max - torso_pos]
if abs(goal_z)<0.005:
#Already at best position, dont try more (avoid moving to noise)
rospy.loginfo("Torso Already at best possible position")
return[False, 0]
#Find best possible case: shoulder @ goal height, max up, or max down.
if goal_z >= spine_range[0] and goal_z <= spine_range[1]:
rospy.loginfo("Goal within spine movement range")
request.ik_request.pose_stamped.pose.position.z = 0;
opt_tor_move = goal_z
elif goal_z > spine_range[1]:#Goal is above possible shoulder height
rospy.loginfo("Goal above spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[1]
opt_tor_move = spine_range[1]
else:#Goal is below possible shoulder height
rospy.loginfo("Goal below spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[0]
opt_tor_move = spine_range[0]
#Check best possible case
print "Optimal Torso move: ", opt_tor_move
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val != 1:
#Return false: Not achievable, even for best case
rospy.loginfo("Original goal cannot be reached")
self.log_out.publish(data="Original goal cannot be reached")
return [False, opt_tor_move]
opt_ik_goal = ik_goal
#Achievable: Find a reasonable solution, move torso, return true
rospy.loginfo("Goal can be reached by moving spine")
self.log_out.publish(data="Goal can be reached by moving spine")
trials = np.linspace(0,opt_tor_move,round(abs(opt_tor_move)/0.05))
np.append(trials,opt_tor_move)
rospy.loginfo("Torso far from best position, finding closer sol.")
print "Trials: ", trials
for trial in trials:
request.ik_request.pose_stamped.pose.position.z = goal_z + trial
print "Trying goal @ ", goal_z + trial
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
rospy.loginfo("Tried: %s, Succeeded" %trial)
return [True, trial]
rospy.loginfo("Tried: %s, Failed" %trial)
#Broke through somehow, catch error
return [True, opt_tor_move]
def full_ik_check(self, ps):
#Check goal as given, if reachable, return true
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
self.dist = pu.calc_dist(self.curr_pose(), ps)
return (True, ik_goal, None)
#Check goal with vertical torso movement, if works, return true
(torso_succeeded, torso_move) = self.check_torso(req)
self.move_torso(self.torso_state.actual.positions[0]+torso_move)
duration = max(4,85*abs(torso_move))
if torso_succeeded:
ik_goal = self.ik_pose_proxy(req)
self.dist = pu.calc_dist(self.curr_pose(), ps)
if ik_goal.error_code.val ==1:
return (True, ik_goal, duration)
else:
print "Reported Succeeded, then really failed: \r\n", ik_goal
#Check goal incrementally retreating hand pose, if works, return true
pct = 0.98
curr_pos = self.curr_pose().pose.position
dx = req.ik_request.pose_stamped.pose.position.x - curr_pos.x
dy = req.ik_request.pose_stamped.pose.position.y - curr_pos.y
while pct > 0.01:
#print "Percent: %s" %pct
req.ik_request.pose_stamped.pose.position.x = curr_pos.x + pct*dx
req.ik_request.pose_stamped.pose.position.y = curr_pos.y + pct*dy
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
rospy.loginfo("Succeeded PARTIALLY (%s) with torso" %pct)
return (True, ik_goal, duration)
else:
pct -= 0.02
#Nothing worked, report failure, return false
rospy.loginfo("IK Failed: Error Code %s" %str(ik_goal.error_code))
rospy.loginfo("Reached as far as possible")
self.log_out.publish(data="Cannot reach farther in this direction.")
return (False, ik_goal, duration)
def form_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = \
self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = \
self.ik_info.kinematic_solver_info.joint_names
req.ik_request.ik_seed_state.joint_state.position = \
self.joint_state_act.positions
return req
def send_joint_angles(self, angles, duration = None):
point = JointTrajectoryPoint()
point.positions = angles
self.send_traj_point(point, duration)
def send_traj_point(self, point, time=None):
if time is None:
point.time_from_start = rospy.Duration(max(20*self.dist, 4))
else:
point.time_from_start = rospy.Duration(time)
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0,0,0,0,0,0,0]
arm_goal = JointTrajectoryGoal()
arm_goal.trajectory = joint_traj
self.arm_traj_client.send_goal(arm_goal)
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_"+self.arm+"_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm+"_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0]+"_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions=[0.01]
pos.constraint_region_orientation = Quaternion(0,0,0,1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
ort = OrientationConstraint()
ort.header.frame_id=goal_in.header.frame_id
ort.link_name = self.arm[0]+"_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving "+self.arm+
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" %state)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning: %s"
%self.move_arm_error_dict[
result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded, pos) = self.check_torso(
self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
%state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
and Torso Check Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
return False
class GazeEstimator(object):
"""This class encapsulates a deep neural network for gaze estimation.
It retrieves two image streams, one containing the left eye and another containing the right eye.
It synchronizes these two images with the estimated head pose.
The images are then converted in a suitable format, and a forward pass (one per eye) of the deep neural network
results in the estimated gaze for this frame. The estimated gaze is then published in the (theta, phi) notation.
Additionally, two images with the gaze overlaid on the eye images are published."""
def __init__(self):
self.image_height = rospy.get_param("~image_height", 36)
self.image_width = rospy.get_param("~image_width", 60)
self.bridge = CvBridge()
self.subjects_bridge = SubjectListBridge()
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
self.use_last_headpose = rospy.get_param("~use_last_headpose", True)
self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
self.last_phi_head, self.last_theta_head = None, None
self.rgb_frame_id_ros = rospy.get_param("~rgb_frame_id_ros",
"/kinect2_nonrotated_link")
self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
self.gpu_id = rospy.get_param("~gpu_id", default="0")
with tensorflow.device("/gpu:{}".format(self.gpu_id)):
config = tensorflow.ConfigProto(inter_op_parallelism_threads=1,
intra_op_parallelism_threads=1)
config.gpu_options.allow_growth = True
config.gpu_options.per_process_gpu_memory_fraction = 0.3
config.log_device_placement = False
self.sess = tensorflow.Session(config=config)
set_session(self.sess)
model_files = rospy.get_param("~model_files")
self.models = []
for model_file in model_files:
tqdm.write('Load model ' + model_file)
model = load_model(os.path.join(
rospkg.RosPack().get_path('rt_gene'), model_file),
custom_objects={
'accuracy_angle': accuracy_angle,
'angle_loss': angle_loss
})
# noinspection PyProtectedMember
model._make_predict_function(
) # have to initialize before threading
self.models.append(model)
tqdm.write('Loaded ' + str(len(self.models)) + ' models')
self.graph = tensorflow.get_default_graph()
self.image_subscriber = rospy.Subscriber('/subjects/images',
MSG_SubjectImagesList,
self.image_callback,
queue_size=3)
self.subjects_gaze_img = rospy.Publisher('/subjects/gazeimages',
Image,
queue_size=3)
self.average_weights = np.array([0.1, 0.125, 0.175, 0.2, 0.4])
self.gaze_buffer_c = {}
def __del__(self):
if self.sess is not None:
self.sess.close()
def estimate_gaze_twoeyes(self, test_input_left, test_input_right,
headpose):
test_headpose = headpose.reshape(1, 2)
with self.graph.as_default():
predictions = []
for model in self.models:
predictions.append(
model.predict({
'img_input_L': test_input_left,
'img_input_R': test_input_right,
'headpose_input': test_headpose
})[0])
mean_prediction = np.mean(np.array(predictions), axis=0)
if len(
self.models
) == 1: # only apply offset for single model, not for ensemble models
mean_prediction[1] += 0.11
return mean_prediction
def visualize_eye_result(self, eye_image, est_gaze):
"""Here, we take the original eye eye_image and overlay the estimated gaze."""
output_image = np.copy(eye_image)
center_x = self.image_width / 2
center_y = self.image_height / 2
endpoint_x, endpoint_y = gaze_tools.get_endpoint(
est_gaze[0], est_gaze[1], center_x, center_y, 50)
cv2.line(output_image, (int(center_x), int(center_y)),
(int(endpoint_x), int(endpoint_y)), (255, 0, 0))
return output_image
def publish_image(self, image, image_publisher, timestamp):
"""This image publishes the `image` to the `image_publisher` with the given `timestamp`."""
image_ros = self.bridge.cv2_to_imgmsg(image, "rgb8")
image_ros.header.stamp = timestamp
image_publisher.publish(image_ros)
def input_from_image(self, eye_img_msg, flip=False):
"""This method converts an eye_img_msg provided by the landmark estimator, and converts it to a format
suitable for the gaze network."""
cv_image = eye_img_msg
#cv_image = self.bridge.imgmsg_to_cv2(eye_img_msg, "rgb8")
if flip:
cv_image = cv2.flip(cv_image, 1)
currimg = cv_image.reshape(self.image_height,
self.image_width,
3,
order='F')
currimg = currimg.astype(np.float32)
# print('currimg.dtype', currimg.dtype)
# cv2.imwrite('/home/tobias/test_inplace.png', currimg)
testimg = np.zeros((1, self.image_height, self.image_width, 3))
testimg[0, :, :, 0] = currimg[:, :, 0] - 103.939
testimg[0, :, :, 1] = currimg[:, :, 1] - 116.779
testimg[0, :, :, 2] = currimg[:, :, 2] - 123.68
return testimg
def compute_eye_gaze_estimation(self, subject_id, timestamp, input_r,
input_l):
"""
subject_id : integer, id of the subject
input_x : cv_image, input image of x eye
(phi_x) : double, phi angle estimated using pupil detection
(theta_x) : double, theta angle estimated using pupil detection
"""
try:
lct = self.tf_listener.getLatestCommonTime(
self.rgb_frame_id_ros, self.headpose_frame + str(subject_id))
if (timestamp - lct).to_sec() < 0.25:
tqdm.write('Time diff: ' + str((timestamp - lct).to_sec()))
(trans_head, rot_head) = self.tf_listener.lookupTransform(
self.rgb_frame_id_ros,
self.headpose_frame + str(subject_id), lct)
euler_angles_head = gaze_tools.get_head_pose(
trans_head, rot_head)
phi_head, theta_head = gaze_tools.get_phi_theta_from_euler(
euler_angles_head)
self.last_phi_head, self.last_theta_head = phi_head, theta_head
else:
if self.use_last_headpose and self.last_phi_head is not None:
tqdm.write('Big time diff, use last known headpose! ' +
str((timestamp - lct).to_sec()))
phi_head, theta_head = self.last_phi_head, self.last_theta_head
else:
tqdm.write(
'Too big time diff for head pose, do not estimate gaze!'
+ str((timestamp - lct).to_sec()))
return
start_time = time.time()
est_gaze_c = self.estimate_gaze_twoeyes(
input_l, input_r, np.array([theta_head, phi_head]))
self.gaze_buffer_c[subject_id].append(est_gaze_c)
if len(self.average_weights) == len(
self.gaze_buffer_c[subject_id]):
est_gaze_c_med = np.average(np.array(
self.gaze_buffer_c[subject_id]),
axis=0,
weights=self.average_weights)
self.publish_gaze(est_gaze_c_med, timestamp, subject_id)
tqdm.write('est_gaze_c: ' + str(est_gaze_c_med))
return est_gaze_c_med
tqdm.write('Elapsed: ' + str(time.time() - start_time))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as tf_e:
print(tf_e)
except rospy.ROSException as ros_e:
if str(ros_e) == "publish() to a closed topic":
print("See ya")
return None
def image_callback(self, subject_image_list, masked_list=None):
"""This method is called whenever new input arrives. The input is first converted in a format suitable
for the gaze estimation network (see :meth:`input_from_image`), then the gaze is estimated (see
:meth:`estimate_gaze`. The estimated gaze is overlaid on the input image (see :meth:`visualize_eye_result`),
and this image is published along with the estimated gaze vector (see :meth:`publish_image` and
:func:`publish_gaze`)"""
timestamp = subject_image_list.header.stamp
subjects_gaze_img = None
subjects_dict = self.subjects_bridge.msg_to_images(subject_image_list)
for subject_id, s in subjects_dict.items():
if subject_id not in self.gaze_buffer_c.keys():
self.gaze_buffer_c[subject_id] = collections.deque(maxlen=5)
input_r = self.input_from_image(s.right, flip=False)
input_l = self.input_from_image(s.left, flip=False)
gaze_est = self.compute_eye_gaze_estimation(
subject_id, timestamp, input_r, input_l)
if gaze_est is not None:
r_gaze_img = self.visualize_eye_result(s.right, gaze_est)
l_gaze_img = self.visualize_eye_result(s.left, gaze_est)
s_gaze_img = np.concatenate((r_gaze_img, l_gaze_img), axis=1)
if subjects_gaze_img is None:
subjects_gaze_img = s_gaze_img
else:
subjects_gaze_img = np.concatenate(
(subjects_gaze_img, s_gaze_img), axis=0)
if subjects_gaze_img is not None:
gaze_img_msg = self.bridge.cv2_to_imgmsg(
subjects_gaze_img.astype(np.uint8), "bgr8")
self.subjects_gaze_img.publish(gaze_img_msg)
def publish_gaze(self, est_gaze, msg_stamp, subject_id):
"""Publish the gaze vector as a PointStamped."""
theta_gaze = est_gaze[0]
phi_gaze = est_gaze[1]
euler_angle_gaze = gaze_tools.get_euler_from_phi_theta(
phi_gaze, theta_gaze)
quaternion_gaze = tf.transformations.quaternion_from_euler(
*euler_angle_gaze)
self.tf_broadcaster.sendTransform(
(0, 0,
0.05), # publish it 5cm above the head pose's origin (nose tip)
quaternion_gaze,
msg_stamp,
self.tf_prefix + "/world_gaze" + str(subject_id),
self.headpose_frame + str(subject_id))
class positionGoalActionServer():
"""
Action server taking position goals and generating twist messages accordingly
"""
def __init__(self,name):
# Get topics and parameters from parameter server
self.max_linear_velocity = rospy.get_param("~max_linear_velocity",2)
self.max_angular_velocity = rospy.get_param("~max_angular_velocity",1)
self.max_distance_error = rospy.get_param("~max_distance_error",0.1)
self.straight_line = rospy.get_param("~straight_line",False)
self.straight_line_angle_error = rospy.get_param("~straight_line_angle_error", 0.5)
#self.odometry_topic = rospy.get_param("~odometry_topic","/base_pose_ground_truth")
self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic","/fmSignals/cmd_vel")
self.odom_frame = rospy.get_param("~odom_frame","/odom")
self.base_frame = rospy.get_param("~base_frame","/base_footprint")
#self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry, self.onOdometry )
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
# Set parameters not yet on server
self.rate = rospy.Rate(5)
# Init variables
self.twist = TwistStamped()
self.destination = vector(0,0)
self.position = vector(0,0)
self.quaternion = np.empty((4, ), dtype=np.float64)
self.distance_error = 0
self.angle_error = 0
self.z = 0
self.w = 0
# Setup TF listener
self.__listen = TransformListener()
# Setup and start simple action server
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name, positionAction, auto_start=False, execute_cb=self.execute)
self._server.register_preempt_callback(self.preempt_cb);
self._server.start()
def preempt_cb(self):
# Publish zero twist message
self.twist.header.stamp = rospy.Time.now()
self.twist.twist.linear.x = 0
self.twist.twist.angular.z = 0
self.twist_pub.publish(self.twist)
#self._server.set_preempted()
def get_current_position(self):
ret = False
try:
(self.position,self.heading) = self.__listen.lookupTransform( self.odom_frame,self.base_frame,rospy.Time(0)) # The transform is returned as position (x,y,z) and an orientation quaternion (x,y,z,w).
ret = True
except (tf.LookupException, tf.ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
return ret
class CF():
def __init__(self, i):
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = 'crazyflie%d' % i
self.zscale = 3
self.state = 0
self.position = []
self.orientation = []
self.pref = []
self.cmd_vel = []
self.goal = PoseStamped()
self.goal.header.seq = 0
self.goal.header.stamp = rospy.Time.now()
pub_name = '/crazyflie%d/goal' % i
sub_name = '/crazyflie%d/CF_state' % i
pub_cmd_diff = '/crazyflie%d/cmd_diff' % i
sub_cmd_vel = '/crazyflie%d/cmd_vel' % i
sub_imu_data = '/crazyflie%d/imu' % i
self.pubGoal = rospy.Publisher(pub_name, PoseStamped, queue_size=1)
self.pubCmd_diff = rospy.Publisher(pub_cmd_diff, Twist, queue_size=1)
self.subCmd_vel = rospy.Subscriber(sub_cmd_vel, Twist,
self.cmdCallback)
self.subGoal = rospy.Subscriber(pub_name, PoseStamped,
self.GoalCallback)
self.subState = rospy.Subscriber(sub_name, String, self.CfsCallback)
self.subImu = rospy.Subscriber(sub_imu_data, Imu, self.ImuCallback)
self.listener = TransformListener()
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
self.updatepos()
self.send_cmd_diff()
def CfsCallback(self, sdata):
self.state = int(sdata.data)
def ImuCallback(self, sdata):
accraw = sdata.linear_acceleration
imuraw = np.array([accraw.x, -accraw.y, -accraw.z])
self.updatepos()
imuraw = cal_R(self.orientation[1], self.orientation[0],
self.orientation[2]).dot(imuraw)
self.Imu = np.array([imuraw[0], imuraw[1], 9.88 + imuraw[2]])
self.Gyro = np.array([
sdata.angular_velocity.x, sdata.angular_velocity.y,
sdata.angular_velocity.z
])
def GoalCallback(self, gdata):
self.goal = gdata
def cmdCallback(self, cdata):
self.cmd_vel = cdata
def hover_init(self, pnext, s):
goal = PoseStamped()
goal.header.seq = self.goal.header.seq + 1
goal.header.frame_id = self.worldFrame
goal.header.stamp = rospy.Time.now()
if self.state != 1:
goal.pose.position.x = pnext[0]
goal.pose.position.y = pnext[1]
goal.pose.position.z = pnext[2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
#print "Waiting for crazyflie %d to take off" %i
else:
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
dx = pnext[0] - position[0]
dy = pnext[1] - position[1]
dz = pnext[2] - position[2]
dq = 0 - rpy[2]
s = max(s, 0.5)
goal.pose.position.x = position[0] + s * dx
goal.pose.position.y = position[1] + s * dy
goal.pose.position.z = position[2] + s * dz
quaternion = tf.transformations.quaternion_from_euler(
0, 0, rpy[2] + s * dq)
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
error = sqrt(dx**2 + dy**2 + dz**2)
print 'Hovering error is %0.2f' % error
if error < 0.1:
return 1
return 0
def updatepos(self):
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
self.position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
self.orientation = rpy
def goto(self, pnext):
goal = PoseStamped()
goal.header.stamp = rospy.Time.now()
goal.header.seq = self.goal.header.seq + 1
goal.header.frame_id = self.worldFrame
goal.pose.position.x = pnext[0]
goal.pose.position.y = pnext[1]
goal.pose.position.z = pnext[2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
def gotoT(self, pnext, s):
goal = PoseStamped()
goal.header.stamp = rospy.Time.now()
goal.header.seq = self.goal.header.seq + 1
goal.header.frame_id = self.worldFrame
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
dx = pnext[0] - position[0]
dy = pnext[1] - position[1]
dz = pnext[2] - position[2]
dq = 0 - rpy[2]
goal.pose.position.x = position[0] + s * dx
goal.pose.position.y = position[1] + s * dy
goal.pose.position.z = position[2] + s * dz
quaternion = tf.transformations.quaternion_from_euler(
0, 0, rpy[2] + s * dq)
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
error = sqrt(dx**2 + dy**2 + dz**2)
print 'error is %0.2f' % error
if error < 0.1:
return 1
else:
return 0
def send_cmd_diff(self, roll=0, pitch=0, yawrate=0, thrust=49000):
# note theoretical default thrust is 39201 equal to 35.89g lifting force
# actual 49000 is 35.89
msg = Twist()
msg.linear.x = -pitch #note vx is -pitch, see crazyflie_server.cpp line 165
msg.linear.y = roll #note vy is roll
msg.linear.z = thrust
msg.angular.z = yawrate
self.pubCmd_diff.publish(msg)
class mbzirc_c2_auto():
# A few key tasks are achieved in the initializer function:
# 1. We load the pre-defined search routine
# 2. We connect to the move_base server in ROS
# 3. We start the ROS subscriber callback function registering the object
# 4. We initialize counters in the class to be shared by the various callback routines
def __init__(self):
# Name this node, it must be unique
rospy.init_node('autonomous', anonymous=True)
# Enable shutdown in rospy (This is important so we cancel any move_base goals
# when the node is killed)
rospy.on_shutdown(self.shutdown)
self.rest_time = rospy.get_param("~rest_time", 0.1) # Minimum pause at each location
self.stalled_threshold = rospy.get_param("~stalled_threshold", 100) # Loops before stall
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
self.waypoints=list()
self.tf = TransformListener()
# self.locations = dict()
# self.wpname = dict()
rospack = rospkg.RosPack()
f = open(rospack.get_path('husky_wp')+'/params/pre-defined-path2.txt','r')
# ct2 = 0
with f as openfileobject:
first_line = f.readline()
for line in openfileobject:
nome = [x.strip() for x in line.split(',')]
#self.wpname[ct2] = nome[0]
x = Decimal(nome[1]); y = Decimal(nome[2]); z = Decimal(nome[3])
X = Decimal(nome[4]); Y = Decimal(nome[5]); Z = Decimal(nome[6])
#self.locations[self.wpname[ct2]] = Pose(Point(x,y,z), Quaternion(X,Y,Z,1))
self.waypoints.append(Pose(Point(x,y,z), Quaternion(0,0,0,1)))
#print self.waypoints
#time.sleep(1)
# ct2 = ct2+1
#self.wp = -1
#self.ct4 = 0
print "Static path has : "
print len(self.waypoints)
print " point(s)."
time.sleep(5)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < len(self.waypoints) and not rospy.is_shutdown():
# Update the marker display
#self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = self.waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
if rospy.has_param('/panel_goal'):
# go to goal the goal goint
# get parameter
panel_goal = rospy.get_param("/panel_goal")
print panel_goal[0];
print panel_goal[1];
print panel_goal[2];
while not rospy.is_shutdown():
try:
(trans,rot) = self.tf.lookupTransform('/odom', '/base_link', rospy.Time(0))
Tx=panel_goal[0]-trans[0];
Ty=panel_goal[1]-trans[1];
Tx=Tx*0.8
Ty=Ty*0.8
goal_x=trans[0]+Tx
goal_y=trans[1]+Ty
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
print "The goal is:"
print goal_x
print goal_y
goal.target_pose.pose = Pose(Point(goal_x,goal_y,0), Quaternion(0,0,0,1))
self.move(goal)
rospy.loginfo("Reach goal")
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("No transformation")
break
else:
rospy.loginfo("No goal found")
def move(self, goal):
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(600))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self._ik_service = IK(side_prefix)
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.marker_cb)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer("ik_request_markers_{}".format(side_prefix))
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert("Move {} arm here".format(side_prefix), callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def get_ee_pose(self):
from_frame = '/base_link'
to_frame = '/' + self.side_prefix + '_wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
except Exception as e:
rospy.logerr('Could not get end effector pose through TF.')
pos = [1.0, 0.0, 1.0]
rot = [0.0, 0.0, 0.0, 1.0]
import traceback
traceback.print_exc()
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def move_to_pose_cb(self, dummy):
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
self.move_to_joints(self.side_prefix, [ik_sol], 1.0)
def marker_clicked_cb(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
self.ee_pose = feedback.pose
self.update_viz()
self._menu_handler.reApply(self._im_server)
self._im_server.applyChanges()
elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Changing visibility of the pose controls.')
if (self.is_control_visible):
self.is_control_visible = False
else:
self.is_control_visible = True
else:
rospy.loginfo('Unhandled event: ' + str(feedback.event_type))
def marker_cb(self, pose_markers):
rospy.loginfo('AR Marker Pose updating')
transform = GripperMarkers.get_matrix_from_pose(pose_markers.markers[0].pose.pose)
offset_array = [0, 0, .03]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
self.ee_pose = GripperMarkers.get_pose_from_transform(hand_transform)
self.update_viz()
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = "x=%.3f y=%.3f z=%.3f" % (pose.position.x, pose.position.y, pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
label_pos = Point()
label_pos.x = pose.position.x
label_pos.y = pose.position.y
label_pos.z = pose.position.z
label = "{} arm".format(self.side_prefix)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=label,
color=ColorRGBA(0.0, 0.0, 0.0, 0.9),
header=Header(frame_id=frame_id),
pose=Pose(label_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def _add_gripper_marker(self, control):
is_hand_open=False
if is_hand_open:
angle = 28 * numpy.pi / 180.0
else:
angle = 0
transform1 = tf.transformations.euler_matrix(0, 0, angle)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
mesh1 = self._make_mesh_marker()
mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
mesh1.pose.position.x = -GripperMarkers._offset
mesh1.pose.orientation.w = 1
mesh2 = self._make_mesh_marker()
mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh3 = self._make_mesh_marker()
mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal)
quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_from_euler(numpy.pi, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, angle))
transform1 = tf.transformations.quaternion_matrix(quat)
transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0]
transform2 = tf.transformations.euler_matrix(0, 0, -angle)
transform2[:3, 3] = [0.09137, 0.00495, 0]
t_proximal = transform1
t_distal = tf.transformations.concatenate_matrices(transform1,transform2)
mesh4 = self._make_mesh_marker()
mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae')
mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal)
mesh5 = self._make_mesh_marker()
mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae')
mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal)
control.markers.append(mesh1)
control.markers.append(mesh2)
control.markers.append(mesh3)
control.markers.append(mesh4)
control.markers.append(mesh5)
return control
@staticmethod
def _offset_pose(pose):
transform = GripperMarkers.get_matrix_from_pose(pose)
offset_array = [-GripperMarkers._offset, 0, 0]
offset_transform = tf.transformations.translation_matrix(offset_array)
hand_transform = tf.transformations.concatenate_matrices(transform,
offset_transform)
return GripperMarkers.get_pose_from_transform(hand_transform)
@staticmethod
def get_matrix_from_pose(pose):
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_pose_from_transform(transform):
pos = transform[:3,3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _add_6dof_marker(self, int_marker):
is_fixed = True
control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed)
int_marker.controls.append(control)
control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed)
int_marker.controls.append(control)
def _make_6dof_control(self, name, orientation, is_move, is_fixed):
control = InteractiveMarkerControl()
control.name = name
control.orientation = orientation
control.always_visible = False
if (self.is_control_visible):
if is_move:
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
else:
control.interaction_mode = InteractiveMarkerControl.NONE
if is_fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
return control
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move += time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
class SprayAction:
"""
Drives x meters either forward or backwards depending on the given distance.
the velocity should always be positive.
"""
def __init__(self, name, odom_frame, base_frame):
"""
@param name: the name of the action
@param odom_frame: the frame the robot is moving in (odom_combined)
@param base_frame: the vehicles own frame (usually base_link)
"""
self._action_name = name
self.__odom_frame = odom_frame
self.__base_frame = base_frame
self.__server = actionlib.SimpleActionServer(self._action_name, sprayAction, auto_start=False)
self.__server.register_preempt_callback(self.preempt_cb)
self.__server.register_goal_callback(self.goal_cb)
self.__cur_pos = 0
self.__start_pos = 0
self.__start_yaw = 0
self.__cur_yaw = 0
self.__feedback = sprayFeedback()
self.__listen = TransformListener()
# self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self.spray_pub = rospy.Publisher("/fmControllers/spray", UInt32)
self.__start_time = rospy.Time.now()
self.new_goal = False
self.n_sprays = 0
self.spray_msg = UInt32()
self.spray_l = False
self.spray_cnt = 0
self.__server.start()
def preempt_cb(self):
rospy.loginfo("Preempt requested")
self.spray_msg.data = 0
self.spray_pub.publish(self.spray_msg)
self.__server.set_preempted()
def goal_cb(self):
"""
called when we receive a new goal
the goal contains a desired radius and a success radius in which we check if the turn succeeded or not
the message also contains if we should turn left or right
"""
g = self.__server.accept_new_goal()
self.spray_dist = g.distance
self.spraytime = g.spray_time
self.new_goal = True
def on_timer(self, e):
"""
called regularly by a ros timer
in here we simply orders the vehicle to start moving either forward
or backwards depending on the distance sign, while monitoring the
travelled distance, if the distance is equal to or greater then this
action succeeds.
"""
if self.__server.is_active():
if self.new_goal:
self.new_goal = False
self.n_sprays = 0
self.__get_start_position()
else:
if self.__get_current_position():
if self.__get_distance() >= self.spray_dist:
self.do_spray()
self.__get_start_position()
else:
self.__server.set_aborted(None, "could not locate")
rospy.logerr("Could not locate vehicle")
self.spray_msg.data = 0
self.spray_pub.publish(self.spray_msg)
def __get_distance(self):
return math.sqrt(
math.pow(self.__cur_pos[0][0] - self.__start_pos[0][0], 2)
+ math.pow(self.__cur_pos[0][1] - self.__start_pos[0][1], 2)
)
def __get_start_position(self):
ret = False
try:
self.__start_pos = self.__listen.lookupTransform(self.__odom_frame, self.__base_frame, rospy.Time(0))
self.__start_yaw = tf.transformations.euler_from_quaternion(self.__start_pos[1])[2]
ret = True
except (tf.LookupException, tf.ConnectivityException), err:
rospy.loginfo("could not locate vehicle")
return ret
class vel_control(object):
def __init__(self, args, joint_values):
self.args = args
self.joint_values_home = joint_values
# CPA PARAMETERS
# At the end, the disciplacement will take place as a final orientation
self.Displacement = [0.01, 0.01, 0.01]
# CPA Parameters
self.zeta = 0.5 # Attractive force gain of the goal
self.max_error_allowed_pos_x = 0.010
self.max_error_allowed_pos_y = 0.010
self.max_error_allowed_pos_z = 0.006
self.max_error_allowed_ori = 0.14
self.dist_att = 0.1 # Influence distance in workspace
self.dist_att_config = 0.2 # Influence distance in configuration space
self.alfa_geral = 1.5 # multiply each alfa (position and rotation) equally
self.gravity_compensation = 9
self.alfa_pos = 4.5 * self.alfa_geral # Grad step of positioning - Default: 0.5
self.alfa_rot = 4 * self.alfa_geral # Grad step of orientation - Default: 0.4
# attributes used to receive msgs while publishing new ones
self.processing = False
self.new_msg = False
self.msg = None
# CPA Parameters
self.diam_goal = 0.05
# Topic used to publish vel commands
self.pub_vel = rospy.Publisher('/joint_group_vel_controller/command', Float64MultiArray, queue_size=10)
# Topic used to control the gripper
self.griper_pos = rospy.Publisher('/gripper/command', JointTrajectory, queue_size=10)
self.gripper_msg = JointTrajectory()
self.gripper_msg.joint_names = ['robotiq_85_left_knuckle_joint']
# visual tools from moveit
# self.scene = PlanningSceneInterface("base_link")
self.marker_publisher = rospy.Publisher('visualization_marker2', Marker, queue_size=10)
# Subscriber used to read joint values
rospy.Subscriber('/joint_states', JointState, self.ur5_actual_position, queue_size=10)
# if true, this node receives messages from publish_dynamic_goal.py
if self.args.dyntest:
# Subscriber used to receive goal coordinates from publish_dynamic_goal.py
rospy.Subscriber('/dynamic_goal', Point, self.get_goal_coordinates, queue_size=10)
# actionClient used to send joint positions
self.client = actionlib.SimpleActionClient('pos_based_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server (pos_based_pos_traj_controller)..."
self.client.wait_for_server()
print "Connected to server (pos_based_pos_traj_controller)"
rospy.sleep(1)
# Standard attributes used to send joint position commands
self.joint_vels = Float64MultiArray()
self.goal = FollowJointTrajectoryGoal()
self.goal.trajectory = JointTrajectory()
self.goal.trajectory.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
'wrist_3_joint']
self.initial_time = 4
# Class attribute used to perform TF transformations
self.tf = TransformListener()
# Denavit-Hartenberg parameters of UR5
# The order of the parameters is d1, SO, EO, a2, a3, d4, d45, d5, d6
self.ur5_param = (0.089159, 0.13585, -0.1197, 0.425, 0.39225, 0.10915, 0.093, 0.09465, 0.0823 + 0.15)
"""
Adds spheres in RVIZ - Used to plot goals and obstacles
"""
def add_sphere(self, pose, diam, color):
marker = Marker()
marker.header.frame_id = "base_link"
marker.id = 0
marker.pose.position = Point(pose[0], pose[1], pose[2])
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = Vector3(diam, diam, diam)
marker.color = color
self.marker_publisher.publish(marker)
"""
Function to ensure safety
"""
def safety_stop(self, ptAtual, wristPt):
# High limit in meters of the end effector relative to the base_link
high_limit = 0.01
# Does not allow wrist_1_link to move above 20 cm relative to base_link
high_limit_wrist_pt = 0.15
if ptAtual[-1] < high_limit or wristPt[-1] < high_limit_wrist_pt:
# Be careful. Only the limit of the end effector is being watched but the other
# joint can also exceed this limit and need to be carefully watched by the operator
rospy.loginfo("High limit of " + str(high_limit) + " exceeded!")
self.home_pos()
raw_input("\n==== Press enter to load Velocity Controller and start APF")
turn_velocity_controller_on()
"""
This function check if the goal position was reached
"""
def all_close(self, goal, tolerance = 0.015):
angles_difference = [self.actual_position[i] - goal[i] for i in range(6)]
total_error = np.sum(angles_difference)
if abs(total_error) > tolerance:
return False
return True
"""
This function is responsible for closing the gripper
"""
def close_gripper(self):
self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.274], velocities=[0], time_from_start=rospy.Duration(0.1))]
self.griper_pos.publish(self.gripper_msg)
"""
This function is responsible for openning the gripper
"""
def open_gripper(self):
self.gripper_msg.points = [JointTrajectoryPoint(positions=[0.0], velocities=[0], time_from_start=rospy.Duration(1.0))]
self.griper_pos.publish(self.gripper_msg)
"""
The joint states published by /joint_staes of the UR5 robot are in wrong order.
/joint_states topic normally publishes the joint in the following order:
[elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
But the correct order of the joints that must be sent to the robot is:
['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
"""
def ur5_actual_position(self, joint_values_from_ur5):
# rospy.loginfo(joint_values_from_ur5)
if self.args.gazebo:
self.th3, self.robotic, self.th2, self.th1, self.th4, self.th5, self.th6 = joint_values_from_ur5.position
else:
self.th3, self.th2, self.th1, self.th4, self.th5, self.th6 = joint_values_from_ur5.position
self.actual_position = [self.th1, self.th2, self.th3, self.th4, self.th5, self.th6]
"""
When to node /dynamic_goal from publish_dynamic_goal.py is used instead of Markers, this
function is responsible for getting the coordinates published by the node and save it
as attribute of the class
"""
def get_goal_coordinates(self, goal_coordinates):
self.ptFinal = [goal_coordinates.x, goal_coordinates.y, goal_coordinates.z]
self.add_sphere(self.ptFinal, self.diam_goal, ColorRGBA(0.0, 1.0, 0.0, 1.0))
"""
Used to test velcoity control under /joint_group_vel_controller/command topic
"""
def velocity_control_test(self):
# publishing rate for velocity control
# Joints are in the order [base, shoulder, elbow, wrist_1, wrist_2, wrist_3]
rate = rospy.Rate(125)
while not rospy.is_shutdown():
self.joint_vels.data = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.pub_vel.publish(self.joint_vels)
rospy.loginfo(self.actual_position)
rate.sleep()
"""
Send the HOME position to the robot
self.client.wait_for_result() does not work well.
Instead, a while loop has been created to ensure that the robot reaches the
goal even after the failure.
"""
def home_pos(self):
turn_position_controller_on()
rospy.sleep(0.1)
print(self.joint_values_home)
# First point is current position
try:
self.goal.trajectory.points = [(JointTrajectoryPoint(positions=self.joint_values_home, velocities=[0]*6, time_from_start=rospy.Duration(self.initial_time)))]
self.initial_time += 1
if not self.all_close(self.joint_values_home):
raw_input("==== Press enter to home the robot!")
print "'Homing' the robot."
self.client.send_goal(self.goal)
self.client.wait_for_result()
while not self.all_close(self.joint_values_home):
self.client.send_goal(self.goal)
self.client.wait_for_result()
except KeyboardInterrupt:
self.client.cancel_goal()
raise
except:
raise
print "\n==== The robot is HOME position!"
"""
Get forces from APF algorithm
"""
def get_joint_forces(self, ptAtual, ptFinal, oriAtual, dist_EOF_to_Goal, err_ori):
# Get UR5 Jacobian of each link
Jacobian = get_geometric_jacobian(self.ur5_param, self.actual_position)
# Getting attractive forces
forces_p = np.zeros((3, 1))
forces_w = np.zeros((3, 1))
for i in range(3):
if abs(ptAtual[i] - ptFinal[i]) <= self.dist_att:
f_att_l = -self.zeta*(ptAtual[i] - ptFinal[i])
else:
f_att_l = -self.dist_att*self.zeta*(ptAtual[i] - ptFinal[i])/dist_EOF_to_Goal[i]
if abs(oriAtual[i] - self.Displacement[i]) <= self.dist_att_config:
f_att_w = -self.zeta*(oriAtual[i] - self.Displacement[i])
else:
f_att_w = -self.dist_att_config*self.zeta*(oriAtual[i] - self.Displacement[i])/dist_EOF_to_Goal[i]
forces_p[i, 0] = f_att_l
forces_w[i, 0] = f_att_w
forces_p = np.asarray(forces_p)
JacobianAtt_p = np.asarray(Jacobian[5])
joint_att_force_p = JacobianAtt_p.dot(forces_p)
joint_att_force_p = np.multiply(joint_att_force_p, [[0.5], [0.1], [1.5], [1], [1], [1]])
forces_w = np.asarray(forces_w)
JacobianAtt_w = np.asarray(Jacobian[6])
joint_att_force_w = JacobianAtt_w.dot(forces_w)
joint_att_force_w = np.multiply(joint_att_force_w, [[0], [0.1], [0.1], [0.4], [0.4], [0.4]])
return np.transpose(joint_att_force_p), np.transpose(joint_att_force_w)
"""
Gets ptFinal and oriAtual
"""
def get_tf_param(self, approach):
# Check if a marker is used instead of a dynamic goal published by publish_dynamic_goal.py
if self.args.armarker:
# When the marker disappears we get an error and the node is killed. To avoid this
# we implemented this try function to check if ar_marker_0 frame is available
try:
# used when Ar Marker is ON
ptFinal, oriFinal = self.tf.lookupTransform("base_link", "ar_marker_0", rospy.Time())
oriFinal = list(euler_from_quaternion(oriFinal))
# Make YAW Angle goes from 0 to 2*pi
# Solution proposed by
# https://answers.ros.org/question/302953/tfquaternion-getangle-eqivalent-for-rospy/
ptAtual, oriAtual = self.tf.lookupTransform("ar_marker_0", "grasping_link", rospy.Time())
angle = -1 * 2 * np.arccos(oriAtual[-1])
oriAtual = list(euler_from_quaternion(oriAtual))
oriAtual[0] = angle
self.add_sphere(ptFinal, self.diam_goal, ColorRGBA(0.0, 1.0, 0.0, 1.0))
if not approach:
# Reach the position above the goal (object)
ptFinal[-1] += 0.02
max_error_allowed_pos_z = self.max_error_allowed_pos_z + ptFinal[-1]
return ptFinal, oriAtual, oriFinal, max_error_allowed_pos_z
max_error_allowed_pos_z = self.max_error_allowed_pos_z
# Return it if pick is performed
return ptFinal, oriAtual, oriFinal, max_error_allowed_pos_z
except:
if not rospy.is_shutdown():
self.home_pos()
raw_input("\nWaiting for /ar_marker_0 frame to be available! Press ENTER after /ar_marker_0 shows up.")
turn_velocity_controller_on()
self.CPA_vel_control(approach)
"""
Main function related the Artificial Potential Field method
"""
def CPA_vel_control(self, approach = False):
# Return the end effector location relative to the base_link
ptAtual, _ = self.tf.lookupTransform("base_link", "grasping_link", rospy.Time())
if approach:
self.alfa_pos = 8 * self.alfa_geral
self.pos_z = 0.04 if approach else None
if self.args.gazebo:
self.alfa_pos = 4.5 * self.alfa_geral * self.gravity_compensation * 0.001 # Grad step of positioning - Default: 0.5
self.alfa_rot = 4 * self.alfa_geral * 0.01
if approach:
self.alfa_pos = 8 * self.alfa_geral * self.gravity_compensation # Grad step of positioning - Default: 0.5
self.alfa_rot = 6 * self.alfa_geral # Grad step of orientation - Default: 0.4
# Get ptFinal published by ar_marker_0 frame and the orientation from grasping_link to ar_marker_0
ptFinal, oriAtual, oriFinal, max_error_allowed_pos_z = self.get_tf_param(approach)
# Calculate the correction of the orientation relative to the actual orientation
R, P, Y = -1 * oriAtual[0], -1 * oriAtual[1], 0.0
corr = [R, P, Y]
oriAtual = [oriAtual[i] + corr[i] for i in range(len(corr))]
err_ori = abs(np.sum(oriAtual))
# Calculate the distance between end effector and goal in each direction
# it is necessary to approach the object
dist_vec_x, dist_vec_y, dist_vec_z = np.abs(ptAtual - np.asarray(ptFinal))
if approach:
dist_EOF_to_Goal = [dist_vec_x, dist_vec_y, self.pos_z]
else:
dist_EOF_to_Goal = [dist_vec_x, dist_vec_y, dist_vec_z]
# Frequency of the velocity controller pubisher
# Max frequency: 125 Hz
rate = rospy.Rate(125)
while not rospy.is_shutdown() and (dist_vec_z > max_error_allowed_pos_z or dist_vec_y > self.max_error_allowed_pos_y or \
dist_vec_x > self.max_error_allowed_pos_x or err_ori > self.max_error_allowed_ori):
# In order to keep orientation constant, we need to correct the orientation
# of the end effector in respect to the ar_marker_0 orientation
oriAtual = [oriAtual[i] + corr[i] for i in range(len(corr))]
# Get absolute orientation error
err_ori = abs(np.sum(oriAtual))
# Get attractive linear and angular forces and repulsive forces
joint_att_force_p, joint_att_force_w = \
self.get_joint_forces(ptAtual, ptFinal, oriAtual, dist_EOF_to_Goal, err_ori)
# Publishes joint valocities related to position only
self.joint_vels.data = np.array(self.alfa_pos * joint_att_force_p[0])
# If orientation control is turned on, sum actual position forces to orientation forces
if self.args.OriON:
self.joint_vels.data = self.joint_vels.data + \
self.alfa_rot * joint_att_force_w[0]
self.pub_vel.publish(self.joint_vels)
# Get ptFinal published by ar_marker_0 frame and the orientation from grasping_link to ar_marker_0
# The oriFinal needs to be tracked online because the object will be dynamic
ptFinal, oriAtual, oriFinal, max_error_allowed_pos_z = self.get_tf_param(approach)
# Calculate the distance between end effector and goal
# dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal))
dist_vec_x, dist_vec_y, dist_vec_z = np.abs(ptAtual - np.asarray(ptFinal))
# Return the end effector position relative to the base_link
ptAtual, _ = self.tf.lookupTransform("base_link", "grasping_link", rospy.Time())
print "Z_position: ", ptAtual[-1]
# Check wrist_1_link position just for safety
wristPt, _ = self.tf.lookupTransform("base_link", "wrist_1_link", rospy.Time())
# Function to ensure safety. It does not allow End Effector to move below 20 cm above the desk
self.safety_stop(ptAtual, wristPt)
if approach:
dist_vec_z = self.pos_z
# The end effector will move 1 cm below the marker
if ptAtual[-1] < (ptFinal[-1] - 0.01):
print "Break loop."
break
ptAtual = [ptAtual[0], ptAtual[1], self.pos_z]
dist_EOF_to_Goal = [dist_vec_x, dist_vec_y, self.pos_z]
else:
dist_EOF_to_Goal = [dist_vec_x, dist_vec_y, dist_vec_z]
rate.sleep()
type=float)
parser.add_argument('--normalize',
action='store_true',
help='scale '
'computed likelihoods (see description)')
args = parser.parse_args()
args = parser.parse_args(rospy.myargv(sys.argv)[1:])
marker_pub = rospy.Publisher('pose_likelihood', Marker)
get_pose_likelihood = rospy.ServiceProxy(
'pose_likelihood_server/'
'get_pose_likelihood', GetMultiplePoseLikelihood)
rospy.sleep(1)
time = tf_listener.getLatestCommonTime('odom', 'base_link')
p, q = tf_listener.lookupTransform('odom', 'base_link', time)
q = quaternion_from_euler(0, 0,
euler_from_quaternion(q)[2] + radians(args.yaw))
def around(base, area, step):
l = arange(base - step, base - area, -step)
r = arange(base, base + area, step)
return hstack([l, r])
x_range = around(p[0], args.area, args.step)
y_range = around(p[1], args.area, args.step)
m = Marker()
m.header.frame_id = 'odom'
m.type = Marker.CUBE_LIST
m.action = Marker.ADD
p2 = Point()
p2.x = humanX
p2.y = humanY
p2.z = 0
marker.points = [p1,p2]
markerArray.markers.append(marker)
publisher.publish(markerArray)
dx = humanX - trans[0]
dy = humanY - trans[1]
target_angle = math.atan2(dy, dx)
rospy.Subscriber("/trackedHumansMod", TrackedHumans, trackedHumans_callback)
print("hi")
# sss.say(["hello"])
# sss.move("base", [1,-0.5,0])
print("ho")
while not rospy.is_shutdown():
# listener1.waitForTransform('/map','/base_link',rospy.Time(),rospy.Duration(5))
if trackedPerson != "":
(trans, rot) = listener.lookupTransform('/map','/base_link',rospy.Time(0))
phi = euler_from_quaternion(rot)
current_angle = phi[2]
"Current angle:"
print phi
# h = sss.move("base", [trans[0], trans[1], target_angle], False)
h = sss.move("base", [trans[0], trans[1], target_angle], False)
rospy.sleep(3)
else:
print "not follow"
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
#self.pidX = PID(20, 12, 0.0, -30, 30, "x")
#self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
self.pidX = PID(20, 12, 0.0, -30, 30, "x")
self.pidY = PID(20, 12, 0.0, -30, 30, "y")
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 1
self.lastJoy = Joy()
def _cmdVelTelopChanged(self, data):
self.cmd_vel_telop = data
if self.state == Controller.Manual:
self.pubNav.publish(data)
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _joyChanged(self, data):
if len(data.buttons) == len(self.lastJoy.buttons):
delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
print("Buton ok")
#Button 1
if delta[0] == 1 and self.state != Controller.Automatic:
print("Automatic!")
thrust = self.cmd_vel_telop.linear.z
print(thrust)
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 1
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.state = Controller.TakeOff
print("TakeOff!")
#Button 5
if delta[4] == 1:
#self.targetZ += 0.1
print(self.targetZ)
#Button 6
if delta[5] == 1:
#self.targetZ -= 0.1
print(self.targetZ)
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark2")
if self.listener.canTransform("/mocap", "/Nano_Mark2", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark2", t)
print(position[0], position[1], position[2])
if position[2] > 0.1 or thrust > 50000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark2")
print(t)
if self.listener.canTransform("/mocap", "/Nano_Mark2", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark2", t)
print(position[0], position[1], position[2])
euler = tf.transformations.euler_from_quaternion(
quaternion)
print(euler[2])
msg = self.cmd_vel_telop
#msg.linear.x = self.pidX.update(0.0,-1.0-position[0])
#msg.linear.y = self.pidY.update(0.0,-1.0-position[1])
msg.linear.z = self.pidZ.update(
0.0, 1.0 - position[2]
) #self.pidZ.update(position[2], self.targetZ)
#msg.angular.z = self.pidYaw.update(euler[2],0.0)
print(msg.linear.x, msg.linear.y, msg.linear.z,
msg.angular.z)
#print(euler[2])
#print(msg.angular.z)
self.pubNav.publish(msg)
rospy.sleep(0.01)
class ArmUtils():
wipe_started = False
standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips)
torso_min = 0.0115
torso_max = 0.295
dist = 0.
move_arm_error_dict = {
-1 : "PLANNING_FAILED: Could not plan a clear path to goal.",
0 : "Move Arm Action Aborted on Internal Failure.",
1 : "SUCCEEDED",
-2 : "TIMED_OUT",
-3 : "START_STATE_IN_COLLISION: Try freeing the arms manually.",
-4 : "START_STATE_VIOLATES_PATH_CONSTRAINTS",
-5 : "GOAL_IN_COLLISION",
-6 : "GOAL_VIOLATES_PATH_CONSTRAINTS",
-7 : "INVALID_ROBOT_STATE",
-8 : "INCOMPLETE_ROBOT_STATE",
-9 : "INVALID_PLANNER_ID",
-10 : "INVALID_NUM_PLANNING_ATTEMPTS",
-11 : "INVALID_ALLOWED_PLANNING_TIME",
-12 : "INVALID_GROUP_NAME",
-13 : "INVALID_GOAL_JOINT_CONSTRAINTS",
-14 : "INVALID_GOAL_POSITION_CONSTRAINTS",
-15 : "INVALID_GOAL_ORIENTATION_CONSTRAINTS",
-16 : "INVALID_PATH_JOINT_CONSTRAINTS",
-17 : "INVALID_PATH_POSITION_CONSTRAINTS",
-18 : "INVALID_PATH_ORIENTATION_CONSTRAINTS",
-19 : "INVALID_TRAJECTORY",
-20 : "INVALID_INDEX",
-21 : "JOINT_LIMITS_VIOLATED",
-22 : "PATH_CONSTRAINTS_VIOLATED",
-23 : "COLLISION_CONSTRAINTS_VIOLATED",
-24 : "GOAL_CONSTRAINTS_VIOLATED",
-25 : "JOINTS_NOT_MOVING",
-26 : "TRAJECTORY_CONTROLLER_FAILED",
-27 : "FRAME_TRANSFORM_FAILURE",
-28 : "COLLISION_CHECKING_UNAVAILABLE",
-29 : "ROBOT_STATE_STALE",
-30 : "SENSOR_INFO_STALE",
-31 : "NO_IK_SOLUTION: Cannot reach goal configuration.",
-32 : "INVALID_LINK_NAME",
-33 : "IK_LINK_IN_COLLISION: Cannot reach goal configuration without contact.",
-34 : "NO_FK_SOLUTION",
-35 : "KINEMATICS_STATE_IN_COLLISION",
-36 : "INVALID_TIMEOUT"
}
def __init__(self, tfListener=None):
self.move_right_arm_client = actionlib.SimpleActionClient('move_right_arm', arm_navigation_msgs.msg.MoveArmAction)
rospy.loginfo("Waiting for move_right_arm server")
if self.move_right_arm_client.wait_for_server(rospy.Duration(50)):
rospy.loginfo("Found move_right_arm server")
else:
rospy.logwarn("Cannot find move_right_arm server")
self.r_arm_traj_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
rospy.loginfo("Waiting for r_arm_controller/joint_trajectory_action server")
if self.r_arm_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found r_arm_controller/joint_trajectory_action server")
else:
rospy.logwarn("Cannot find r_arm_controller/joint_trajectory_action server")
self.r_arm_follow_traj_client = actionlib.SimpleActionClient('r_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
rospy.loginfo("Waiting for r_arm_controller/follow_joint_trajectory server")
if self.r_arm_follow_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found r_arm_controller/follow_joint_trajectory server")
else:
rospy.logwarn("Cannot find r_arm_controller/follow_joint_trajectory server")
self.torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
rospy.loginfo("Waiting for torso_controller/position_joint_action server")
if self.torso_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found torso_controller/position_joint_action server")
else:
rospy.logwarn("Cannot find torso_controller/position_joint_action server")
self.r_gripper_client = actionlib.SimpleActionClient('r_gripper_controller/gripper_action', Pr2GripperCommandAction)
rospy.loginfo("Waiting for r_gripper_controller/gripper_action server")
if self.r_gripper_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found r_gripper_controller/gripper_action server")
else:
rospy.logwarn("Cannot find r_gripper_controller/gripper_action server")
rospy.loginfo("Waiting for r_utility_frame_service")
try:
rospy.wait_for_service('/r_utility_frame_update', 7.0)
self.update_frame = rospy.ServiceProxy('/r_utility_frame_update', FrameUpdate)
rospy.loginfo("Found r_utility_frame_service")
except:
rospy.logwarn("Right Utility Frame Service Not available")
#self.joint_state_lock = RLock()
rospy.Subscriber('r_arm_controller/state', JointTrajectoryControllerState, self.update_joint_state)
self.torso_state_lock = RLock()
rospy.Subscriber('torso_controller/state', JointTrajectoryControllerState, self.update_torso_state)
#self.r_arm_command = rospy.Publisher('/r_arm_controller/command', JointTrajectory )
self.wt_log_out = rospy.Publisher('wt_log_out', String)
if tfListener is None:
self.tf = TransformListener()
else:
self.tf = tfListener
self.tfb = TransformBroadcaster()
rospy.loginfo("Waiting for FK/IK Solver services")
try:
rospy.wait_for_service('/pr2_right_arm_kinematics/get_fk')
rospy.wait_for_service('/pr2_right_arm_kinematics/get_fk_solver_info')
rospy.wait_for_service('/pr2_right_arm_kinematics/get_ik')
rospy.wait_for_service('/pr2_right_arm_kinematics/get_ik_solver_info')
self.fk_info_proxy = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_fk_solver_info',
GetKinematicSolverInfo)
self.fk_info = self.fk_info_proxy()
self.fk_pose_proxy = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_fk', GetPositionFK, True)
self.ik_info_proxy = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_ik_solver_info',
GetKinematicSolverInfo)
self.ik_info = self.ik_info_proxy()
self.ik_pose_proxy = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_ik', GetPositionIK, True)
rospy.loginfo("Found FK/IK Solver services")
except:
rospy.logerr("Could not find FK/IK Solver services")
self.set_planning_scene_diff_name = '/environment_server/set_planning_scene_diff'
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
self.set_planning_scene_diff = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
self.set_planning_scene_diff(SetPlanningSceneDiffRequest())
def update_joint_state(self, jtcs):
#self.joint_state_lock.acquire()
self.joint_state_time = jtcs.header.stamp
self.joint_state_act = jtcs.actual
self.joint_state_des = jtcs.desired
#self.joint_state_lock.release()
def update_torso_state(self, jtcs):
self.torso_state_lock.acquire()
self.torso_state = jtcs
self.torso_state_lock.release()
def curr_pose(self):
#print 'Requesting Current Pose'
(trans,rot) = self.tf.lookupTransform('/torso_lift_link', '/r_wrist_roll_link', rospy.Time(0))
cp = PoseStamped()
cp.header.stamp = rospy.Time.now()
cp.header.frame_id = '/torso_lift_link'
cp.pose.position = Point(*trans)
cp.pose.orientation = Quaternion(*rot)
#print 'Current Pose:', cp
return cp
def wait_for_stop(self, wait_time=1, timeout=60):
rospy.sleep(wait_time)
end_time = rospy.Time.now()+rospy.Duration(timeout)
while not self.is_stopped():
if rospy.Time.now()<end_time:
rospy.sleep(wait_time)
else:
raise
def is_stopped(self):
#self.joint_state_lock.acquire()
time = self.joint_state_time
vels = self.joint_state_act.velocities
#self.joint_state_lock.release()
if np.allclose(vels, np.zeros(7)) and (rospy.Time.now()-time)<rospy.Duration(0.1):
return True
else:
return False
def get_fk(self, angles, frame='torso_lift_link', link=-1): # get FK pose of a list of joint angles for the arm
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = frame
fk_request.header.stamp = rospy.Time.now()
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.header = fk_request.header
fk_request.robot_state.joint_state.position = angles #self.joint_state_act.positions
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
print 'fk_request:', fk_request
try:
return self.fk_pose_proxy(fk_request).pose_stamped[link]
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" %str(e))
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose) / 2])
pose_right.append(pose[len(pose) / 2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(
pose_left, pose_right, left_gripper_states,
right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav,
self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10, 0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230, 0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80, 0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100, 0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout()
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150, 0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150, 0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [
Pose(Point(2.04748392105, 2.04748010635, 0.000),
Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0),
Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000),
Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000),
Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000),
Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))
]
self.index = 1
rospy.loginfo("Completed GUI initialization")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text())
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' %
(old_arm_state, arm_side))
elif ('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[
self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[
self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif ('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif ('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(
self.get_joint_state('l'), self.get_joint_state('r'),
self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif ('Move to Bin' == button_name):
self.move_to_bin_action()
elif ('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while (self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if (map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif ('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point=None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose,
map_point.header.frame_id,
'/base_link')
rospy.loginfo("Object is " + str(map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False
if (map_point.pose.position.x < 0.7):
move_back_first = True
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose,
'/base_link', '/map')
if (move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if (success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations[
'ready_pickup'].left
self.animPlay.right_poses = self.saved_animations[
'ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations[
'ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range(0, 3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined',
self.base_frame,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0], trans[0][1], trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait=False):
self.torso_client = SimpleActionClient(
'/torso_controller/position_joint_action',
SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(
rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Torso movement goal failed with error code: " +
str(state))
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations[
'left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations[
'l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(
re.sub(
',', '',
str(self.animation_map[pose_name].left).strip('[]') +
' ' +
str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(
str(self.animation_map[pose_name].left_gripper) + ' ' +
str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(
self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(
self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(
l_animation_queue, r_animation_queue, l_gripper_queue,
r_gripper_queue)
self.saved_animations_list.addItem(
animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz(
"%sing %s Gripper" %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText(
'%s %s Gripper' %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if ('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif ('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif ('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class MarkerProcessor(object):
def __init__(self, use_dummy_transform=False):
rospy.init_node("star_center_positioning_node")
self.robot_name = rospy.get_param("~robot_name", "")
self.odom_frame_name = self.robot_name + "_odom"
# print self.odom_frame_name
self.marker_locators = {}
self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))
self.pose_correction = rospy.get_param("~pose_correction", 0.0)
self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10)
self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10)
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
def add_marker_locator(self, marker_locator):
self.marker_locators[marker_locator.id] = marker_locator
def process_odom(self, msg):
p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name), pose=msg.pose.pose)
try:
STAR_pose = self.tf_listener.transformPose("STAR", p)
STAR_pose.header.stamp = msg.header.stamp
self.continuous_pose.publish(STAR_pose)
except Exception as inst:
print "error is", inst
def process_markers(self, msg):
for marker in msg.markers:
# do some filtering basd on prior knowledge
# we know the approximate z coordinate and that all angles but yaw should be close to zero
euler_angles = euler_from_quaternion(
(
marker.pose.pose.orientation.x,
marker.pose.pose.orientation.y,
marker.pose.pose.orientation.z,
marker.pose.pose.orientation.w,
)
)
angle_diffs = (
TransformHelpers.angle_diff(euler_angles[0], pi),
TransformHelpers.angle_diff(euler_angles[1], 0),
)
print angle_diffs, marker.pose.pose.position.z
if (
marker.id in self.marker_locators
and 3.0 <= marker.pose.pose.position.z <= 3.6
and fabs(angle_diffs[0]) <= 0.4
and fabs(angle_diffs[1]) <= 0.4
):
print "FOUND IT!"
locator = self.marker_locators[marker.id]
xy_yaw = list(locator.get_camera_position(marker))
print "xy_yaw", xy_yaw
xy_yaw[0] += self.pose_correction * cos(xy_yaw[2])
xy_yaw[1] += self.pose_correction * sin(xy_yaw[2])
print xy_yaw
orientation_tuple = quaternion_from_euler(0, 0, xy_yaw[2])
pose = Pose(
position=Point(x=-xy_yaw[0], y=-xy_yaw[1], z=0),
orientation=Quaternion(
x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]
),
)
# TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
pose_stamped = PoseStamped(header=Header(stamp=rospy.Time.now(), frame_id="STAR"), pose=pose)
try:
offset, quaternion = self.tf_listener.lookupTransform(
self.robot_name + "_base_link", self.robot_name + "_base_laser_link", rospy.Time(0)
)
except Exception as inst:
print "Error", inst
return
# TODO: use frame timestamp instead of now()
pose_stamped_corrected = deepcopy(pose_stamped)
pose_stamped_corrected.pose.position.x -= offset[0] * cos(xy_yaw[2])
pose_stamped_corrected.pose.position.y -= offset[0] * sin(xy_yaw[2])
self.star_pose_pub.publish(pose_stamped_corrected)
self.fix_STAR_to_odom_transform(pose_stamped_corrected)
def fix_STAR_to_odom_transform(self, msg):
""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose)
p = PoseStamped(
pose=TransformHelpers.convert_translation_rotation_to_pose(translation, rotation),
header=Header(stamp=rospy.Time(), frame_id=self.robot_name + "_base_link"),
)
try:
self.tf_listener.waitForTransform(
self.robot_name + "_odom", self.robot_name + "_base_link", rospy.Time(), rospy.Duration(1.0)
)
except Exception as inst:
print "whoops", inst
return
print "got transform"
self.odom_to_STAR = self.tf_listener.transformPose(self.robot_name + "_odom", p)
(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map to odom transformation.
This is necessary so things like move_base can work properly. """
if not (hasattr(self, "translation") and hasattr(self, "rotation")):
return
self.tf_broadcaster.sendTransform(
self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR"
)
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.broadcast_last_transform()
r.sleep()
tTC = proxy.getTransform(chainName, space, False)
print "Transform %s to %s:"%(space_to_str(space),chainName)
print transform_to_str(tTCSensor)
print
T = np.matrix([tTCSensor[0:4],tTCSensor[4:8],tTCSensor[8:12],tTCSensor[12:]])
#print cam_rotation_matrix()
DCM = T*cam_rotation_matrix()
print "Transform %s to %s (with rotated coordinate frame):"%(space_to_str(space),chainName)
print np_mat_to_str(DCM)
stamp = rospy.Time()
frame1 = 'Torso_link'
frame2 = 'CameraTop_frame'
try:
listener.waitForTransform(frame1,frame2,stamp,rospy.Duration(1.0))
(trans,rot) = listener.lookupTransform(frame1,frame2,stamp)
except tf.Exception as e:
print "ERROR using TF"
print "%s"%(e)
sys.exit(-1)
m = transformations.quaternion_matrix(rot)
for i in range(0,3):
m[i,3] = trans[i]
print "[tf] Transform %s to %s:"%(frame1,frame2)
print np_mat_to_str(m)
e = np.linalg.norm(DCM - m)
print "Error is: ",e
if e > 1e-5:
print "ERROR: Something is wrong with your TF transformations. Transforms do not match!"
class NavigateInRowSimple():
"""
Executor to navigate in a row given an fmMsgs/row message
This executor navigates in a row given a fmMsgs/row message
the row is navigated based on the desired offset from the row.
This executor publishes a cmd_vel directly.
If the row message received is too old the executor aborts.
If a transform between the robot and odometry frame exists
then the distance driven in the row is returned as a result.
"""
__feedback_msg = navigate_in_row_simpleFeedback()
__goal_msg = navigate_in_row_simpleResult()
def __init__(self,name,rowtopic,odom_frame,vehicle_frame):
"""
Initialise the action server
@param name: the name of the action server to start
@param rowtopic: the topic id of the row message to use for navigating
@param odom_frame: the frame in which the robot is moving
@param vehicle_frame: the frame id of the vehicles base
"""
## Setup some default values even though they are received via the goal
self.desired_offset = 0.3
self.max_out_of_headland_count = 10
self.pgain = 0.2
self.distance_scale = 0.1
self.forward_velocity= 0.5
# store the odometry frame id for use in distance calculation
self.odom_frame = odom_frame
self.vehicle_frame = vehicle_frame
# reset counters used for detecting loss of rows and headland
self.outofheadlandcount = 0
self.left_valid_count = 0
self.right_valid_count = 0
self.__listen = TransformListener()
self.cur_row = None
self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self._action_name = name
self._server = actionlib.SimpleActionServer(self._action_name,navigate_in_row_simpleAction,auto_start=False)
self._server.register_goal_callback(self.goal_cb)
self._server.register_preempt_callback(self.preempt_cb);
self._subscriber = rospy.Subscriber(rowtopic, row, callback=self.row_msg_callback)
self._last_row_msg = None
self._server.start()
self._timer = rospy.Timer(rospy.Duration(0.1),self.on_timer)
def preempt_cb(self):
"""
Called when the action client wishes to preempt us.
Currently we just publish a velocity of zero in order to stop the vehicle.
"""
rospy.loginfo("preempt received")
self.__send_safe_velocity()
self._server.set_preempted()
def goal_cb(self):
"""
Called when we receive a new goal.
We could inspect the goal before accepting it,
however for start we only accept it.
"""
rospy.loginfo("goal received")
g = self._server.accept_new_goal()
self.desired_offset = g.desired_offset_from_row
self.max_out_of_headland_count = g.headland_timeout
self.pgain = g.P
self.distance_scale = g.distance_scale
self.forward_velocity= g.forward_velcoity
self.outofheadlandcount = 0
# reset start and end pose
self.__start_pose = self.get_pose()
self.__end_pose = None
self.left_valid_count = self.right_valid_count = 0
self.start_time = rospy.Time.now()
def row_msg_callback(self,row):
"""
Stores the row received
"""
self.cur_row = row
def get_pose(self):
"""
returns the pose of the vehicle in the odometry frame
returns none if the vehicle could not be located
"""
cur_pos = None
try:
cur_pos = self.__listen.lookupTransform(self.odom_frame,self.vehicle_frame,rospy.Time(0))
except (LookupException, ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
return cur_pos
class Pointmass_Adjust:
pub_marker = True #Set to (True) or to not(False) publish rviz marker showing force vector
mass = 1.0463 #kg 1.0213
pos_x = 0.0853 #m, in 'l_wrist_roll_link'
pos_y = 0.0
pos_z = 0.0
x_force_offset = 5.62 #5.70 -- These values determined from experiment, used values are adjusted for better qualitative results using rviz
y_force_offset = -13.56 #14.10
z_force_offset = 4.30 #-3.88
x_torque_offset = -0.4025 #0.3899
y_torque_offset = -0.4175 #0.3804
z_torque_offset = -0.21875
adjustment = WrenchStamped()
def __init__(self):
rospy.init_node("ft_pointmass_adjustment")
rospy.Subscriber("netft_data", WrenchStamped, self.adjust)
self.ft_out = rospy.Publisher('ft_data_pm_adjusted', WrenchStamped)
self.force_vec_out = rospy.Publisher('ft_force_vector_marker', Marker)
self.tfl = TransformListener()
def adjust(self, ft_in):
ft_out = WrenchStamped()
ft_out.header.stamp = rospy.Time.now()
ft_out.header.frame_id = ft_in.header.frame_id
ft_out.wrench.force.x = ft_in.wrench.force.x - self.x_force_offset + self.adjustment.wrench.force.x
ft_out.wrench.force.y = ft_in.wrench.force.y - self.y_force_offset + self.adjustment.wrench.force.y
ft_out.wrench.force.z = ft_in.wrench.force.z - self.z_force_offset + self.adjustment.wrench.force.z
ft_out.wrench.torque.x = ft_in.wrench.torque.x - self.x_torque_offset - self.adjustment.wrench.torque.x
ft_out.wrench.torque.y = ft_in.wrench.torque.y - self.y_torque_offset - self.adjustment.wrench.torque.y
ft_out.wrench.torque.z = ft_in.wrench.torque.z - self.z_torque_offset - self.adjustment.wrench.torque.z
self.ft_out.publish(ft_out)
if self.pub_marker:
marker = self.form_marker(ft_out)
self.force_vec_out.publish(marker)
def form_marker(self, ws):
origin = Point()
force_point = Point()
force_point.x = 0.1 * ws.wrench.force.x
force_point.y = 0.1 * ws.wrench.force.y
force_point.z = 0.1 * ws.wrench.force.z
force_vec = Marker()
force_vec.header.stamp = rospy.Time.now()
force_vec.header.frame_id = '/l_netft_frame'
force_vec.ns = "ft_sensor"
force_vec.action = 0
force_vec.type = 0
force_vec.scale.x = 0.1
force_vec.scale.y = 0.2
force_vec.scale.z = 1
force_vec.color.a = 0.75
force_vec.color.r = 0.0
force_vec.color.g = 1.0
force_vec.color.b = 0.1
force_vec.lifetime = rospy.Duration(1)
force_vec.points.append(origin)
force_vec.points.append(force_point)
return force_vec
def calc_adjustment(self):
try:
(pos, quat) = self.tfl.lookupTransform('/base_link',
'/l_netft_frame',
rospy.Time(0))
except:
return
rot = transformations.euler_from_quaternion(quat)
self.adjustment.wrench.torque.x = self.mass * 9.80665 * self.pos_x * math.sin(
rot[0])
self.adjustment.wrench.torque.y = self.mass * 9.80665 * self.pos_x * math.sin(
rot[1])
self.adjustment.wrench.torque.z = 0
grav = PointStamped(
) # Generate a 'vector' of the force due to gravity at the ft sensor
grav.header.stamp = rospy.Time(
0
) #Used to tell tf to grab latest available transform in transformVector3
grav.header.frame_id = '/base_link'
grav.point.x = pos[0]
grav.point.y = pos[1]
grav.point.z = pos[2] - 9.80665 * self.mass
netft_grav = self.tfl.transformPoint(
'/l_netft_frame', grav
) #Returns components of the 'gravity force' in each axis of the 'l_netft_frame'
self.adjustment.wrench.force.x = netft_grav.point.x
self.adjustment.wrench.force.y = netft_grav.point.y
self.adjustment.wrench.force.z = netft_grav.point.z
self.adjustment.header.stamp = rospy.Time.now()
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
#self.pidX = PID(20, 12, 0.0, -30, 30, "x")
#self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
#self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
#self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.pidX = PID(20, 12.0, 0.2, -30, 30, "x")
self.pidY = PID(-20, -12.0, -0.2, -30, 30, "y")
#50000 800
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 57000, "z")
self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 1
self.targetX = 0.0
self.targetY = -1.0
self.des_angle = 90.0
self.lastZ = 0.0
self.power = 50000.0
self.pubVz = rospy.Publisher('vel_z', Float32, queue_size=1)
self.lastJoy = Joy()
def _cmdVelTelopChanged(self, data):
self.cmd_vel_telop = data
if self.state == Controller.Manual:
self.pubNav.publish(data)
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _joyChanged(self, data):
if len(data.buttons) == len(self.lastJoy.buttons):
delta = np.array(data.buttons) - np.array(self.lastJoy.buttons)
print ("Buton ok")
#Button 1
if delta[0] == 1 and self.state != Controller.Automatic:
print("Automatic!")
#thrust = self.cmd_vel_telop.linear.z
#print(thrust)
self.pidReset()
self.pidZ.integral = self.power/self.pidZ.ki
self.lastZ = 0.0
#self.targetZ = 1
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.state = Controller.TakeOff
print("TakeOff!")
#Button 5
if delta[4] == 1:
self.targetY = 0.0
self.des_angle = -90.0
#print(self.targetZ)
#self.power += 100.0
print(self.power)
#self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
self.targetY = -1.0
self.des_angle = 90.0
#print(self.targetZ)
#self.power -= 100.0
print(self.power)
#self.state = Controller.Automatic
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark3")
if self.listener.canTransform("/mocap", "/Nano_Mark3", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark3", t)
print(position[0],position[1],position[2])
if position[2] > 0.2 or thrust > 54000:
self.pidReset()
#self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark3")
seconds = rospy.get_time()
print(t)
if self.listener.canTransform("/mocap","/Nano_Mark3", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark3",t)
#print(position[0],position[1],position[2])
euler = tf.transformations.euler_from_quaternion(quaternion)
#print(euler[2]*(180/math.pi))
msg = self.cmd_vel_telop
#print(self.power)
if self.lastZ == 0.0:
self.lastZ = position[2]
last_time = seconds;
else:
dh = self.targetZ - position[2]
v_z = (position[2]-self.lastZ)/((seconds-last_time))
#print(v_z)
self.pubVz.publish(v_z)
#por encima de goal
if dh<0.0:
self.power -=50
#if v_z>0.0:
# self.power -=50
#else:
#self.power +=50
#por debajo de goal
if dh>0.0:
if self.power > 57000.0:
self.power = 57000.0
else:
self.power += 50
#if v_z<0.0:
# self.power +=50
# else:
# self.power -=50
print(self.power)
msg.linear.z = self.power
#Descompostion of the x and y contributions following the Z-Rotation
x_prim = self.pidX.update(0.0, self.targetX-position[0])
y_prim = self.pidY.update(0.0,self.targetY-position[1])
msg.linear.x = x_prim*math.cos(euler[2]) - y_prim*math.sin(euler[2])
msg.linear.y = x_prim*math.sin(euler[2]) + y_prim*math.cos(euler[2])
#---old stuff---
#msg.linear.x = self.pidX.update(0.0, 0.0-position[0])
#msg.linear.y = self.pidY.update(0.0,-1.0-position[1])
#msg.linear.z = self.pidZ.update(position[2],1.0)
#z_prim = self.pidZ.update(0.0,self.targetZ-position[2])
#print(z_prim)
#if z_prim < self.power:
# msg.linear.z = self.power
#else:
# msg.linear.z = z_prim
#msg.linear.z = z_prim
#msg.linear.z = self.pidZ.update(0.0,1.0-position[2]) #self.pidZ.update(position[2], self.targetZ)
#msg.angular.z = self.pidYaw.update(0.0,self.des_angle + euler[2]*(180/math.pi))
print(msg.linear.x,msg.linear.y,msg.linear.z,msg.angular.z)
#print(euler[2])
#print(msg.angular.z)
self.pubNav.publish(msg)
rospy.sleep(0.01)
class Circle():
def __init__(self, goals):
rospy.init_node('circle', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.radius = rospy.get_param("~radius")
self.freq = rospy.get_param("~freq")
self.x = rospy.get_param("~x")
self.y = rospy.get_param("~y")
self.z = rospy.get_param("~z")
self.lap = rospy.get_param("~lap")
self.omega = self.freq * 2 * 3.1415926
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 0
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
#rospy.loginfo("start running!")
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
while not rospy.is_shutdown():
#rospy.loginfo("start running!")
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.x
goal.pose.position.y = self.y
goal.pose.position.z = self.z
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if math.fabs(position[0] - self.x) < 0.15 \
and math.fabs(position[1] - self.y) < 0.15 \
and math.fabs(position[2] - self.z) < 0.15 \
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \
and self.goalIndex < len(self.goals) - 2:
rospy.sleep(2)
self.goalIndex += 1
#rospy.loginfo("Index:%lf",self.goalIndex)
if self.goalIndex == len(self.goals) - 2:
break
t_start = rospy.Time.now().to_sec()
#rospy.loginfo("t_start:%lf",t_start)
t_now = t_start
while ((not rospy.is_shutdown())
and (t_now - t_start < self.lap / self.freq)):
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.x + self.radius * math.sin(
(t_now - t_start) * self.omega)
goal.pose.position.y = self.y + self.radius - self.radius * math.cos(
(t_now - t_start) * self.omega)
goal.pose.position.z = self.z
quaternion = tf.transformations.quaternion_from_euler(
0, 0, (t_now - t_start) * self.omega)
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t_now = rospy.Time.now().to_sec()
#rospy.loginfo("t_now-t_start:%lf",t_now-t_start)
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.x
goal.pose.position.y = self.y
goal.pose.position.z = self.z
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
#rospy.loginfo("Index:%lf",self.goalIndex)
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if math.fabs(position[0] - self.x) < 0.15 \
and math.fabs(position[1] - self.y) < 0.15 \
and math.fabs(position[2] - self.z) < 0.15 \
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \
and self.goalIndex < len(self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
class SocialGaze:
def __init__(self):
self.defaultLookatPoint = Point(1,0,1.35)
self.downLookatPoint = Point(0.5,0,0.5)
self.targetLookatPoint = Point(1,0,1.35)
self.currentLookatPoint = Point(1,0,1.35)
self.currentGazeAction = GazeGoal.LOOK_FORWARD;
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint);
# Some constants
self.doNotInterrupt = [GazeGoal.GLANCE_RIGHT_EE, GazeGoal.GLANCE_LEFT_EE, GazeGoal.NOD, GazeGoal.SHAKE];
self.nodPositions = [Point(1,0,1.05), Point(1,0,1.55)]
self.shakePositions = [Point(1,0.2,1.35), Point(1,-0.2,1.35)]
self.nNods = 5
self.nShakes = 5
self.nodCounter = 5
self.shakeCounter = 5
self.facePos = None
## Action client for sending commands to the head.
self.headActionClient = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self.headActionClient.wait_for_server()
rospy.loginfo('Head action client has responded.')
self.headGoal = PointHeadGoal()
self.headGoal.target.header.frame_id = 'base_link'
self.headGoal.min_duration = rospy.Duration(1.0)
self.headGoal.target.point = Point(1,0,1)
## Client for receiving detected faces
#self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction)
## Service client for arm states
self.tfListener = TransformListener()
## Server for gaze requested gaze actions
self.gazeActionServer = actionlib.SimpleActionServer('gaze_action', GazeAction, self.executeGazeAction, False)
self.gazeActionServer.start()
## Callback function to receive ee states and face location
def getEEPos(self, armIndex):
fromFrame = '/base_link'
if (armIndex == 0):
toFrame = '/r_wrist_roll_link'
else:
toFrame = '/l_wrist_roll_link'
try:
t = self.tfListener.getLatestCommonTime(fromFrame, toFrame)
(position, rotation) = self.tfListener.lookupTransform(fromFrame, toFrame, t)
except:
rospy.logerr('Could not get the end-effector pose.')
#objPoseStamped = PoseStamped()
#objPoseStamped.header.stamp = t
#objPoseStamped.header.frame_id = '/base_link'
#objPoseStamped.pose = Pose()
#relEEPose = self.tfListener.transformPose(toFrame, objPoseStamped)
return Point(position[0], position[1], position[2])
def getFaceLocation(self):
connected = self.faceClient.wait_for_server(rospy.Duration(1.0))
if connected:
fgoal = FaceDetectorGoal()
self.faceClient.send_goal(fgoal)
self.faceClient.wait_for_result()
f = self.faceClient.get_result()
## If there is one face follow that one, if there are more than one, follow the closest one
closest = -1
if len(f.face_positions) == 1:
closest = 0
elif len(f.face_positions) > 0:
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y + f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
self.facePos = array([f.face_positions[closest].pos.x, f.face_positions[closest].pos.y, f.face_positions[closest].pos.z])
else:
rospy.logwarn('No faces were detected.')
self.facePos = self.defaultLookatPoint
else:
rospy.logwarn('Not connected to the face server, cannot follow faces.')
self.facePos = self.defaultLookatPoint
## Callback function for receiving gaze commands
def executeGazeAction(self, goal):
command = goal.action;
if (self.doNotInterrupt.count(self.currentGazeAction) == 0):
if (self.currentGazeAction != command or command == GazeGoal.LOOK_AT_POINT):
self.isActionComplete = False
if (command == GazeGoal.LOOK_FORWARD):
self.targetLookatPoint = self.defaultLookatPoint
elif (command == GazeGoal.LOOK_DOWN):
self.targetLookatPoint = self.downLookatPoint
elif (command == GazeGoal.NOD):
self.nNods = 5
self.startNod()
elif (command == GazeGoal.SHAKE):
self.nShakes = 5
self.startShake()
elif (command == GazeGoal.NOD_ONCE):
self.nNods = 5
self.startNod()
elif (command == GazeGoal.SHAKE_ONCE):
self.nShakes = 5
self.startShake()
elif (command == GazeGoal.GLANCE_RIGHT_EE):
self.startGlance(0)
elif (command == GazeGoal.GLANCE_LEFT_EE):
self.startGlance(1)
elif (command == GazeGoal.LOOK_AT_POINT):
self.targetLookatPoint = goal.point
rospy.loginfo('Setting gaze action to:' + str(command))
self.currentGazeAction = command
while (not self.isActionComplete):
time.sleep(0.1)
self.gazeActionServer.set_succeeded()
else:
self.gazeActionServer.set_aborted()
else:
self.gazeActionServer.set_aborted()
def isTheSame(self, current, target):
diff = target - current
dist = linalg.norm(diff)
return (dist<0.0001)
def filterLookatPosition(self, current, target):
speed = 0.02
diff = self.point2array(target) - self.point2array(current)
dist = linalg.norm(diff)
if (dist>speed):
step = dist/speed
return self.array2point(self.point2array(current) + diff/step)
else:
return target
def startNod(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.nodCounter = 0
self.targetLookatPoint = self.nodPositions[0]
def startGlance(self, armIndex):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.glanceCounter = 0
self.targetLookatPoint = self.getEEPos(armIndex)
def startShake(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.shakeCounter = 0
self.targetLookatPoint = self.shakePositions[0]
def point2array(self, p):
return array((p.x, p.y, p.z))
def array2point(self, a):
return Point(a[0], a[1], a[2])
def getNextNodPoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.nodCounter += 1
if (self.nodCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.nodPositions[self.nodCounter%2]
else:
return target
def getNextGlancePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.glanceCounter = 1
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return target
def getNextShakePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.shakeCounter += 1
if (self.shakeCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.shakePositions[self.shakeCounter%2]
else:
return target
def update(self):
isActionPossiblyComplete = True
if (self.currentGazeAction == GazeGoal.FOLLOW_RIGHT_EE):
self.targetLookatPoint = self.getEEPos(0)
elif (self.currentGazeAction == GazeGoal.FOLLOW_LEFT_EE):
self.targetLookatPoint = self.getEEPos(1)
elif (self.currentGazeAction == GazeGoal.FOLLOW_FACE):
self.getFaceLocation()
self.targetLookatPoint = self.facePos
elif (self.currentGazeAction == GazeGoal.NOD):
self.targetLookatPoint = self.getNextNodPoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == GazeGoal.SHAKE):
self.targetLookatPoint = self.getNextShakePoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == GazeGoal.GLANCE_RIGHT_EE or self.currentGazeAction == GazeGoal.GLANCE_LEFT_EE):
self.targetLookatPoint = self.getNextGlancePoint(self.currentLookatPoint, self.targetLookatPoint)
isActionPossiblyComplete = False;
self.currentLookatPoint = self.filterLookatPosition(self.currentLookatPoint, self.targetLookatPoint)
if (self.isTheSame(self.point2array(self.headGoal.target.point), self.point2array(self.currentLookatPoint))):
if (isActionPossiblyComplete):
if (self.headActionClient.get_state() == GoalStatus.SUCCEEDED):
self.isActionComplete = True
else:
self.headGoal.target.point = self.currentLookatPoint
self.headActionClient.send_goal(self.headGoal)
time.sleep(0.02)
class Crazyflie:
def __init__(self, prefix, cf_id, use_tf=False):
"""
Creates a Crazyflie high-level wrapper
Args:
prefix (str): ROS namespace of the drone. Ex = "/cf1"
cf_id (int): drone id. Ex : 1
"""
self.prefix = prefix
if use_tf:
self.tf = TransformListener()
self.cf_id = cf_id
rospy.wait_for_service(prefix + "/set_group_mask")
self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask)
rospy.wait_for_service(prefix + "/takeoff")
self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
rospy.wait_for_service(prefix + "/land")
self.landService = rospy.ServiceProxy(prefix + "/land", Land)
rospy.wait_for_service(prefix + "/stop")
self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
rospy.wait_for_service(prefix + "/go_to")
self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
rospy.wait_for_service(prefix + "/upload_trajectory")
self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory)
rospy.wait_for_service(prefix + "/start_trajectory")
self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory)
rospy.wait_for_service(prefix + "/update_params")
self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams)
def setGroup(self, groupMask):
self.setGroupMaskService(groupMask)
def takeoff(self, targetHeight, duration, groupMask = 0):
self.takeoffService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
def land(self, targetHeight, duration, groupMask = 0):
self.landService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
def stop(self, groupMask = 0):
self.stopService(groupMask)
def goTo(self, goal, yaw, duration, relative = False, groupMask = 0):
gp = arrayToGeometryPoint(goal)
self.goToService(groupMask, relative, gp, yaw, rospy.Duration.from_sec(duration))
def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rospy.Duration.from_sec(poly.duration)
piece.poly_x = poly.px.p
piece.poly_y = poly.py.p
piece.poly_z = poly.pz.p
piece.poly_yaw = poly.pyaw.p
pieces.append(piece)
self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)
def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative)
def position(self):
if not self.tf:
raise RuntimeError("CF instance was created without using tf. set use_tf=True to get position")
self.tf.waitForTransform("/world", "/cf" + str(self.cf_id), rospy.Time(0), rospy.Duration(10))
position, quaternion = self.tf.lookupTransform("/world", "/cf" + str(self.cf_id), rospy.Time(0))
return np.array(position)
def getParam(self, name):
return rospy.get_param(self.prefix + "/" + name)
def setParam(self, name, value):
rospy.set_param(self.prefix + "/" + name, value)
self.updateParamsService([name])
def setParams(self, params):
for name, value in params.iteritems():
rospy.set_param(self.prefix + "/" + name, value)
self.updateParamsService(params.keys())
def enableHighLevel(self):
self.setParam("commander/enHighLevel", 1)
class TurnAction():
"""
Performs a X degree turn either to the left or right
depending on the given goal.
"""
def __init__(self,name,odom_frame,base_frame):
"""
@param name: the name of the action
@param odom_frame: the frame the robot is moving in (odom_combined)
@param base_frame: the vehicles own frame (usually base_link)
"""
self._action_name = name
self.__odom_frame = odom_frame
self.__base_frame = base_frame
self.__server = actionlib.SimpleActionServer(self._action_name,make_turnAction,auto_start=False)
self.__server.register_preempt_callback(self.preempt_cb)
self.__server.register_goal_callback(self.goal_cb)
self.__cur_pos = 0
self.__start_yaw = 0
self.__cur_yaw = 0
self.__feedback = make_turnFeedback()
self.__listen = TransformListener()
self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist)
self.__turn_timeout = 200
self.__start_time = rospy.Time.now()
self.turn_vel = 0
self.new_goal = False
self.__server.start()
def preempt_cb(self):
rospy.loginfo("Preempt requested")
self.__publish_cmd_vel(0)
self.__server.set_preempted()
def goal_cb(self):
"""
called when we receive a new goal
the goal contains a desired radius and a success radius in which we check if the turn succeeded or not
the message also contains if we should turn left or right
"""
g = self.__server.accept_new_goal()
self.__desired_amount= g.amount
self.turn_vel = g.vel
self.forward_vel = g.forward_vel
self.__cur_pos = None
self.__cur_yaw = 0
self.__start_yaw = 0
self.new_goal = True
def on_timer(self,e):
"""
called regularly by a ros timer
This function exevutes the main loop of this action
if a goal is active a rabbit is placed initially at the desired distance
from the robot at either left or right.
"""
if self.__server.is_active():
if self.new_goal:
self.new_goal = False
if self.__get_start_position():
self.__start_time = rospy.Time.now()
else:
self.__server.set_aborted(text="could not find vehicle")
else:
if rospy.Time.now() - self.__start_time > rospy.Duration(self.__turn_timeout):
self.__server.set_aborted(text="timeout on action")
self.__publish_cmd_vel(0)
else:
if self.__get_current_position():
if self.__desired_amount > 0:
if self.compare_yaw_turn(self.__start_yaw,self.__cur_yaw, self.__desired_amount):
result = make_turnResult()
result.end_yaw = self.__cur_yaw
self.__server.set_succeeded(result, "turn completed")
self.__publish_cmd_vel(0)
else:
self.__publish_cmd_vel(1)
else:
# notice swap of position and call of yaw
if self.compare_yaw_turn(self.__cur_yaw, self.__start_yaw, self.__desired_amount*-1):
result = make_turnResult()
result.end_yaw = self.__cur_yaw
self.__server.set_succeeded(result, "turn completed")
self.__publish_cmd_vel(0)
else:
self.__publish_cmd_vel(1)
self.__feedback.start = self.__start_yaw
self.__feedback.current = self.__cur_yaw
self.__feedback.target= self.__start_yaw + self.__desired_amount
self.__server.publish_feedback(self.__feedback)
else:
self.__publish_cmd_vel(0)
def compare_yaw_turn(self,start,current,amount):
diff = current - start
if diff > math.pi:
diff -= 2 * math.pi
elif diff < - math.pi:
diff += 2 * math.pi
self.__feedback.start = start
self.__feedback.current = current
self.__feedback.target = diff
self.__server.publish_feedback(self.__feedback)
if diff >= amount:
return True
else:
return False
def __get_start_position(self):
ret = False
try:
self.__start_pos = self.__listen.lookupTransform(self.__odom_frame,self.__base_frame,rospy.Time(0))
self.__start_yaw = tf.transformations.euler_from_quaternion(self.__start_pos[1])[2]
ret = True
except (tf.LookupException, tf.ConnectivityException),err:
rospy.loginfo("could not locate vehicle")
return ret
class AccurateDocking(RComponent):
"""
A class used to make a simple docking process
"""
def __init__(self):
RComponent.__init__(self)
self.step = -1
# array of {'initial': [x,y,theta], 'final': [x,y,theta]}
self.results = []
self.ongoing_result = {}
self.total_iterations = 0
self.stop = False
def ros_read_params(self):
"""Gets params from param server"""
RComponent.ros_read_params(self)
self.dock_action_name = rospy.get_param('~dock_action_server',
default='pp_docker')
self.move_action_name = rospy.get_param('~move_action_server',
default='move')
self.robot_link = rospy.get_param('~base_frame',
default='robot_base_link')
self.pregoal_link = rospy.get_param(
'~pregoal_frame', default='laser_test_pregoal_footprint')
self.goal_link = rospy.get_param('~goal_frame',
default='laser_test_goal_footprint')
self.reflectors_link = rospy.get_param(
'~reflectors_frame',
default='robot_filtered_docking_station_laser')
# array of [x, y, theta] for the first dock call
param_offset_1 = rospy.get_param('~pregoal_offset_1',
default='[-0.5, 0, 0]')
self.pregoal_offset_1 = (param_offset_1.replace('[', '').replace(
']', '').replace(',', '')).split(' ')
if len(self.pregoal_offset_1) != 3:
self.pregoal_offset_1 = [0, 0, 0]
else:
for i in range(len(self.pregoal_offset_1)):
self.pregoal_offset_1[i] = float(self.pregoal_offset_1[i])
# array of [x, y, theta] for the second dock call
param_offset_2 = rospy.get_param('~pregoal_offset_2',
default='[-0.0, 0, 0]')
self.pregoal_offset_2 = (param_offset_2.replace('[', '').replace(
']', '').replace(',', '')).split(' ')
if len(self.pregoal_offset_2) != 3:
self.pregoal_offset_2 = [0, 0, 0]
else:
for i in range(len(self.pregoal_offset_2)):
self.pregoal_offset_2[i] = float(self.pregoal_offset_2[i])
rospy.loginfo("%s::ros_read_params: docker offsets: %s - %s",
rospy.get_name(), str(self.pregoal_offset_1),
str(self.pregoal_offset_2))
self.step_back_distance = rospy.get_param('~step_back_distance',
default=1.3)
self.consecutive_iterations = rospy.get_param(
'~consecutive_iterations', default=1)
self.current_iteration = self.consecutive_iterations
def ros_setup(self):
"""Creates and inits ROS components"""
RComponent.ros_setup(self)
self.tf = TransformListener()
self.move_action_client = actionlib.SimpleActionClient(
self.move_action_name, MoveAction)
self.dock_action_client = actionlib.SimpleActionClient(
self.dock_action_name, DockAction)
self.start_docking_server = rospy.Service(
'~start', Trigger, self.start_docking_service_cb)
self.stop_docking_server = rospy.Service('~stop', Trigger,
self.stop_docking_service_cb)
self.save_results_server = rospy.Service('~save_results', Empty,
self.save_results_service_cb)
self.docking_status = rospy.Publisher('~status', String, queue_size=10)
return 0
def ready_state(self):
"""Actions performed in ready state"""
if self.current_iteration < self.consecutive_iterations:
self.docking_status.publish("running")
if self.step != -1:
try:
# Get relative goal position
t = self.tf.getLatestCommonTime(self.robot_link,
self.goal_link)
(position, quaternion) = self.tf.lookupTransform(
self.robot_link, self.goal_link, t)
(_, _, orientation) = euler_from_quaternion(quaternion)
except Exception, e:
rospy.logerr("%s::ready_state: exception: %s",
rospy.get_name(), e)
self.stop = True
return
try:
# Get relative goal to the reflectors position
t = self.tf.getLatestCommonTime(self.robot_link,
self.reflectors_link)
(position_to_reflectors,
quaternion_to_reflectors) = self.tf.lookupTransform(
self.robot_link, self.reflectors_link, t)
(_, _, orientation_to_reflectors
) = euler_from_quaternion(quaternion_to_reflectors)
except Exception, e:
rospy.logerr("%s::ready_state: exception: %s",
rospy.get_name(), e)
error_on_action()
return
if self.step == 0:
self.ongoing_result = {}
rospy.loginfo('%s::ros_setup: waiting for server %s',
rospy.get_name(), self.move_action_name)
self.move_action_client.wait_for_server()
rospy.loginfo('%s::ros_setup: waiting for server %s',
rospy.get_name(), self.dock_action_name)
self.dock_action_client.wait_for_server()
rospy.loginfo(
'%s::ready_state: Initial distance to goal-> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position[0] * 1000, position[1] * 1000,
math.degrees(orientation))
self.ongoing_result['initial'] = [
position[0], position[1],
math.degrees(orientation)
]
# Docking
dock_goal = DockGoal()
dock_goal.dock_frame = self.pregoal_link
dock_goal.robot_dock_frame = self.robot_link
dock_goal.dock_offset.x = self.pregoal_offset_1[0]
dock_goal.dock_offset.y = self.pregoal_offset_1[1]
dock_goal.dock_offset.theta = self.pregoal_offset_1[2]
rospy.loginfo(
'%s::ready_state: %d - docking to %s - offset = %s',
rospy.get_name(), self.step, self.pregoal_link,
str(dock_goal.dock_offset))
self.dock_action_client.send_goal(dock_goal)
self.dock_action_client.wait_for_result()
result = self.dock_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
self.step = 1
rospy.sleep(2)
else:
rospy.logerr("%s::ready_state: Docking failed",
rospy.get_name())
error_on_action()
elif self.step == 1:
# Docking
dock_goal = DockGoal()
dock_goal.dock_frame = self.pregoal_link
dock_goal.robot_dock_frame = self.robot_link
dock_goal.dock_offset.x = self.pregoal_offset_2[0]
dock_goal.dock_offset.y = self.pregoal_offset_2[1]
dock_goal.dock_offset.theta = self.pregoal_offset_2[2]
rospy.loginfo(
'%s::ready_state: %d - docking to %s - offset = %s',
rospy.get_name(), self.step, self.pregoal_link,
str(dock_goal.dock_offset))
self.dock_action_client.send_goal(dock_goal)
self.dock_action_client.wait_for_result()
result = self.dock_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
self.step = 2
else:
rospy.logerr("%s::ready_state: Docking failed",
rospy.get_name())
error_on_action()
elif self.step == 2:
# Orientate robot
move_goal = MoveGoal()
move_goal.goal.theta = orientation
if abs(math.degrees(orientation)) > 0.2:
rospy.loginfo(
'%s::ready_state: %d - rotating %.3lf degrees to %s',
rospy.get_name(), self.step, math.degrees(orientation),
self.goal_link)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step,
str(result.success))
if result.success == True:
self.step = 3
else:
rospy.logerr("%s::ready_state: Move-Rotation failed",
rospy.get_name())
error_on_action()
else:
rospy.loginfo(
'%s::ready_state: %d - avoids rotating %.3lf degrees to %s',
rospy.get_name(), self.step, math.degrees(orientation),
self.goal_link)
self.step = 3
elif self.step == 3:
# Move forward
move_goal = MoveGoal()
move_goal.goal.x = position[0]
rospy.loginfo(
'%s::ready_state: %d - moving forward %.3lf meters to %s',
rospy.get_name(), self.step, move_goal.goal.x,
self.goal_link)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
rospy.sleep(2)
self.step = 4
else:
rospy.logerr("%s::ready_state: Move-Forward failed",
rospy.get_name())
error_on_action()
elif self.step == 4:
rospy.loginfo(
'%s::ready_state: final distance to goal -> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position[0] * 1000, position[1] * 1000,
math.degrees(orientation))
rospy.loginfo(
'%s::ready_state: final distance to reflectors -> x: %.3lf mm, y: %.3lf mm, %.3lf degrees',
rospy.get_name(), position_to_reflectors[0] * 1000,
position_to_reflectors[1] * 1000,
math.degrees(orientation_to_reflectors))
self.ongoing_result['final'] = [
position[0], position[1],
math.degrees(orientation)
]
self.results.append(self.ongoing_result)
if self.step_back_distance == 0.0:
self.iteration_success()
else:
self.step = 5
elif self.step == 5:
# Move backward
move_goal = MoveGoal()
move_goal.goal.x = -self.step_back_distance
rospy.loginfo(
'%s::ready_state: %d - moving backwards %.3lf meters',
rospy.get_name(), self.step, move_goal.goal.x)
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
result = self.move_action_client.get_result()
rospy.loginfo('%s::ready_state: %d - result = %s',
rospy.get_name(), self.step, str(result.success))
if result.success == True:
rospy.sleep(2)
self.iteration_success()
else:
rospy.logerr("%s::ready_state: Move-Forward failed",
rospy.get_name())
self.error_on_action()
class MergePoses:
def __init__(self):
self.avg_pose = None
self.tl = TransformListener()
self.topics = rospy.get_param('~poses',[])
print self.topics
if len(self.topics) == 0:
rospy.logerr('Please provide a poses list as input parameter')
return
self.output_pose = rospy.get_param('~output_pose','ar_avg_pose')
self.output_frame = rospy.get_param('~output_frame', 'rf_map')
self.subscribers = []
for i in self.topics:
subi = rospy.Subscriber(i, PoseStamped, self.callback, queue_size=1)
self.subscribers.append(subi)
self.pub = rospy.Publisher(self.output_pose, PoseStamped, queue_size=1)
self.mutex_avg = threading.Lock()
self.mutex_t = threading.Lock()
self.transformations = {}
def callback(self, pose_msg):
# get transformation to common frame
position = None
quaternion = None
if self.transformations.has_key(pose_msg.header.frame_id):
position, quaternion = self.transformations[pose_msg.header.frame_id]
else:
if self.tl.frameExists(pose_msg.header.frame_id) and \
self.tl.frameExists(self.output_frame):
t = self.tl.getLatestCommonTime(self.output_frame, pose_msg.header.frame_id)
position, quaternion = self.tl.lookupTransform(self.output_frame,
pose_msg.header.frame_id, t)
self.mutex_t.acquire()
self.transformations[pose_msg.header.frame_id] = (position, quaternion)
self.mutex_t.release()
rospy.loginfo("Got static transform for %s" % pose_msg.header.frame_id)
# transform pose
framemat = self.tl.fromTranslationRotation(position, quaternion)
posemat = self.tl.fromTranslationRotation([pose_msg.pose.position.x,
pose_msg.pose.position.y,
pose_msg.pose.position.z],
[pose_msg.pose.orientation.x,
pose_msg.pose.orientation.y,
pose_msg.pose.orientation.z,
pose_msg.pose.orientation.w])
newmat = numpy.dot(framemat,posemat)
xyz = tuple(translation_from_matrix(newmat))[:3]
quat = tuple(quaternion_from_matrix(newmat))
tmp_pose = PoseStamped()
tmp_pose.header.stamp = pose_msg.header.stamp
tmp_pose.header.frame_id = self.output_frame
tmp_pose.pose = Pose(Point(*xyz),Quaternion(*quat))
tmp_angles = euler_from_quaternion([tmp_pose.pose.orientation.x,
tmp_pose.pose.orientation.y,
tmp_pose.pose.orientation.z,
tmp_pose.pose.orientation.w])
# compute average
self.mutex_avg.acquire()
if self.avg_pose == None or (pose_msg.header.stamp - self.avg_pose.header.stamp).to_sec() > 0.5:
self.avg_pose = tmp_pose
else:
self.avg_pose.header.stamp = pose_msg.header.stamp
a = 0.7
b = 0.3
self.avg_pose.pose.position.x = a*self.avg_pose.pose.position.x + b*tmp_pose.pose.position.x
self.avg_pose.pose.position.y = a*self.avg_pose.pose.position.y + b*tmp_pose.pose.position.y
self.avg_pose.pose.position.z = a*self.avg_pose.pose.position.z + b*tmp_pose.pose.position.z
angles = euler_from_quaternion([self.avg_pose.pose.orientation.x,
self.avg_pose.pose.orientation.y,
self.avg_pose.pose.orientation.z,
self.avg_pose.pose.orientation.w])
angles = list(angles)
angles[0] = avgAngles(angles[0], tmp_angles[0], 0.7, 0.3)
angles[1] = avgAngles(angles[1], tmp_angles[1], 0.7, 0.3)
angles[2] = avgAngles(angles[2], tmp_angles[2], 0.7, 0.3)
q = quaternion_from_euler(angles[0], angles[1], angles[2])
self.avg_pose.pose.orientation.x = q[0]
self.avg_pose.pose.orientation.y = q[1]
self.avg_pose.pose.orientation.z = q[2]
self.avg_pose.pose.orientation.w = q[3]
self.pub.publish(self.avg_pose)
self.mutex_avg.release()
class StatePredictionNode:
""" This class contains the methods for state prediction """
def __init__(self):
rospy.init_node('odometry', anonymous=True)
self.dt = .02
self.control_voltages = np.zeros([2, 1])
self.vl_filter = MedianFilter()
self.vr_filter = MedianFilter()
self.vb_filter = MedianFilter()
self.vl = 0 # left wheel velocity (after filter)
self.vr = 0 # right wheel velocity (after filter)
self.vb = 0 # base angular velocity (after filter)
self.base_imu_ready = False # base imu is slower than the other IMU's
# we can't update base velocity all the time
Q = np.zeros([7, 7])
Q[0, 0] = 1 * self.dt
Q[1, 1] = 1 * self.dt
R = np.eye(3) * .00001
starting_state = np.zeros([7, 1])
self.r = .15
self.l = .55
self.ekf = ExtendedWMRKalmanFilter(self.r, self.l, Q, R, 2.3364e-04,
-.65, 2.3364e-04, -.65,
starting_state, self.dt)
self.listener = TransformListener()
self.i = 0 # print index
self.j = 0 # move camera index
self.imus_observed_list = []
self.ar_observed_list = []
self.sensed_ar_diff = {
} # map from tag_id sensed to array of np.array([dx, dy, dtheta_z])
self.landmark_map = {} # map from id to AR TAG obj
## ADD PREDEFINED MAPS
#(tag_num, slam_id, x, y, z, theta_x, theta_y, theta_z, fixed = True)
self.landmark_map[0] = ARTAG_landmark(0, 1, 0, 1.37, .10, math.pi / 2,
0, 0)
self.landmark_map[1] = ARTAG_landmark(13, 2, -1.74, 0, .15,
math.pi / 2, 0, math.pi / 2)
self.landmark_map[3] = ARTAG_landmark(5, 3, 2.84, .15, .32,
math.pi / 2, 0, -math.pi / 2)
self.landmark_map[2] = ARTAG_landmark(9, 4, 1.37, 1.88, .06,
math.pi / 2, 0, 0)
self.ekf.add_AR_tag(self.landmark_map[0], np.zeros([3, 3]))
self.ekf.add_AR_tag(self.landmark_map[1], np.zeros([3, 3]))
self.ekf.add_AR_tag(self.landmark_map[2], np.zeros([3, 3]))
self.ekf.add_AR_tag(self.landmark_map[3], np.zeros([3, 3]))
self.br = tf.TransformBroadcaster()
## FILE WRITING INFO
self.f = open('ekf_data.csv', 'w+')
self.f.write('time, vl,vr,vx,vy,vtheta,x,y,theta,raw_vl,raw_vr\n')
self.t0 = time.time()
self.transform_written = False
self.write_imu_measurements = [0, 0, 0]
self.pub = rospy.Publisher('odometry', Odometry, queue_size=1)
rospy.Subscriber("/imu1", Imu, self._imu1Callback)
rospy.Subscriber("/imu2", Imu, self._imu2Callback)
rospy.Subscriber("/imu3", Imu, self._imu3Callback)
rospy.Subscriber("left_voltage_pwm", Int16, self._leftMotorCallback)
rospy.Subscriber("right_voltage_pwm", Int16, self._rightMotorCallback)
rospy.Subscriber("ar_pose_marker", AlvarMarkers, self._arCallback)
rospy.Subscriber("scan", LaserScan, self._scanCallback)
self.last_time = rospy.Time.now()
def _scanCallback(self, data):
return
def _leftMotorCallback(self, data):
self.control_voltages[0] = data.data
def _rightMotorCallback(self, data):
self.control_voltages[1] = data.data
def _imu1Callback(self, data):
self.imus_observed_list.append('imu1')
left_wheel_v = -data.angular_velocity.x
self.write_imu_measurements[0] = left_wheel_v
if abs(left_wheel_v) < .05:
self.vl_filter.add_data(0)
self.vl = self.vl_filter.get_median()
return
self.vl_filter.add_data(left_wheel_v)
self.vl = self.vl_filter.get_median()
def _imu2Callback(self, data):
self.imus_observed_list.append('imu2')
right_wheel_v = -data.angular_velocity.x
self.write_imu_measurements[1] = right_wheel_v
if abs(right_wheel_v) < .05:
self.vr_filter.add_data(0)
self.vr = self.vr_filter.get_median()
return
self.vr_filter.add_data(right_wheel_v)
self.vr = self.vr_filter.get_median()
def _imu3Callback(self, data):
self.imus_observed_list.append('imu3')
self.vb = data.angular_velocity.z - .01
self.write_imu_measurements[2] = self.vb
if abs(self.vb) < .05:
self.vb = 0
return
def _arCallback(self, data):
for marker in data.markers:
if marker.id in self.landmark_map:
tag_frame = 'ar_marker_' + str(marker.id)
t = self.listener.getLatestCommonTime(tag_frame, 'base')
p_tr, q_tr = self.listener.lookupTransform(
tag_frame, 'base', t)
H_tr = self.listener.fromTranslationRotation(p_tr, q_tr)
# IGNORE READING IF IT IS TOO FAR AWAY
if np.linalg.norm(p_tr) > 2.5:
return
t = self.listener.getLatestCommonTime(tag_frame + '_ref',
'odom')
p_ot, q_ot = self.listener.lookupTransform(
'odom', tag_frame + '_ref', t)
H_ot = self.listener.fromTranslationRotation(p_ot, q_ot)
H_ot = np.dot(H_ot, H_tr)
angles = transformations.euler_from_matrix(H_ot[0:3, 0:3],
axes='sxyz')
self.sensed_ar_diff[marker.id] = np.array([[H_ot[0, 3]],
[H_ot[1, 3]],
[angles[2]]])
self.ar_observed_list.append(
marker.id) # we observed an AR tag!
return
def _publish_odom(self):
state = copy.deepcopy(self.ekf.current_state_estimate)
## BUILD AND SEND MSG
quaternion = tf.transformations.quaternion_from_euler(0, 0, state[6])
msg = Odometry()
msg.header.stamp = rospy.Time.now()
msg.pose.pose.position.x = state[4]
msg.pose.pose.position.y = state[5]
msg.pose.pose.position.z = 0
msg.pose.pose.orientation.x = quaternion[0]
msg.pose.pose.orientation.y = quaternion[1]
msg.pose.pose.orientation.z = quaternion[2]
msg.pose.pose.orientation.w = quaternion[3]
msg.twist.twist.linear.x = state[2] * math.cos(state[6])
msg.twist.twist.linear.y = state[2] * math.sin(state[6])
msg.twist.twist.linear.z = 0
msg.twist.twist.angular.z = state[3]
self.pub.publish(msg)
def _update_with_imus(self):
if len(self.imus_observed_list) == 0:
return
# Initalize intermediary variables
measurement_list = []
H_list = []
R_list = []
num_states = self.ekf.current_state_estimate.shape[0]
## CHECK EACH SENSOR
if 'imu1' in self.imus_observed_list:
H_row = np.zeros([1, num_states])
H_row[0, 0] = 1
H_list.append(H_row)
measurement_list.append(self.vl)
R_list.append(.05)
if 'imu2' in self.imus_observed_list:
H_row = np.zeros([1, num_states])
H_row[0, 1] = 1
H_list.append(H_row)
measurement_list.append(self.vr)
R_list.append(.05)
if 'imu3' in self.imus_observed_list:
H_row = np.zeros([1, num_states])
H_row[0, 3] = 1
H_list.append(H_row)
measurement_list.append(self.vb)
R_list.append(4e-5)
# BUILD H
H = H_list[0]
del H_list[0]
for h_block in H_list:
H = np.vstack((H, h_block))
# BUILD MEASUREMENTS
measurements = measurement_list[0]
del measurement_list[0]
for measurement in measurement_list:
measurements = np.vstack((measurements, measurement))
# BUILD R
R = np.zeros([H.shape[0], H.shape[0]])
r_index = 0 # element of diagonal to fill
for r_elem in R_list:
R[r_index, r_index] = r_elem
r_index += 1
# reset things
self.imus_observed_list = []
self.ekf.linear_measurement_update(measurements, H, R)
# record imu measurements used to update kalman filter to write to a file
def _update_with_landmarks(self):
if len(self.ar_observed_list) == 0:
return
# CHECK ALL LANDMARKS
for landmark_id in self.landmark_map.keys():
if landmark_id in self.ar_observed_list:
measurement = self.sensed_ar_diff[landmark_id]
H = np.zeros([3, self.ekf.current_prob_estimate.shape[0]])
H[:, 4:7] = np.eye(3)
dist = np.linalg.norm(measurement[0:2])
R = np.eye(3) * dist**3 * .1
self.ekf.linear_measurement_update(measurement, H, R, True)
#print measurement
# reset things
self.ar_observed_list = []
self.sensed_ar_diff = {}
def _publish_tf(self):
# ROBOT TRANSFORM
self.last_time = rospy.Time.now()
state = copy.deepcopy(self.ekf.current_state_estimate)
for landmark_id in self.landmark_map.keys():
ar_obj = self.landmark_map[landmark_id]
self.br.sendTransform((ar_obj.x, ar_obj.y, ar_obj.z),
tf.transformations.quaternion_from_euler(ar_obj.theta_x, ar_obj.theta_y, ar_obj.theta_z),\
self.last_time,\
"ar_marker_" + str(ar_obj.tag_num) + "_ref",\
"odom")
self.br.sendTransform((state[4], state[5], 0),\
tf.transformations.quaternion_from_euler(0, 0, state[6]),\
self.last_time,\
"base_link",\
"odom")
self.br.sendTransform(
(0, 0, .1), tf.transformations.quaternion_from_euler(0, 0, 0),
self.last_time, "laser", "base_link")
self.br.sendTransform(
(0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),
self.last_time, "base", "base_link")
return
def _update_ekf(self):
self.ekf.predict(self.control_voltages)
self._update_with_imus()
self._update_with_landmarks()
def _print(self):
""" Print info to the screen every once and a while """
self.i += 1
np.set_printoptions(precision=3, suppress=True)
if self.i % 40 == 0:
self.i = 0
print self.ekf.current_state_estimate[4:7]
def _write_to_file(self):
state = copy.deepcopy(self.ekf.current_state_estimate)
self.f.write(
str(time.time() - self.t0) + ',' + str(state[0, 0]) + ',' +
str(state[1, 0]) + ',' + str(state[2, 0]) + ',' +
str(state[3, 0]) + ',' + str(state[4, 0]) + ',' +
str(state[5, 0]) + ',' + str(state[6, 0]) + ',' +
str(state[7, 0]) + ',' + str(self.write_imu_measurements[0]) +
',' + str(self.write_imu_measurements[1]) + ',' +
str(self.write_imu_measurements[2]) + '\n')
def spin(self):
r = rospy.Rate(1 / self.dt)
while not rospy.is_shutdown():
self._publish_tf()
self._update_ekf()
self._print()
self._publish_odom()
self._write_to_file()
self.control_voltages[0] = 0
self.control_voltages[1] = 0
r.sleep()
type=float)
parser.add_argument('--step', help='discretization step in meters '
'(default: 0.05)', default=0.05, type=float)
parser.add_argument('--normalize', action='store_true', help='scale '
'computed likelihoods (see description)')
args = parser.parse_args()
args = parser.parse_args(rospy.myargv(sys.argv)[1:])
marker_pub = rospy.Publisher('pose_likelihood', Marker)
get_pose_likelihood = rospy.ServiceProxy('pose_likelihood_server/'
'get_pose_likelihood',
GetMultiplePoseLikelihood)
rospy.sleep(1)
time = tf_listener.getLatestCommonTime('odom', 'base_link')
p, q = tf_listener.lookupTransform('odom', 'base_link', time)
q = quaternion_from_euler(0, 0, euler_from_quaternion(q)[2] +
radians(args.yaw))
def around(base, area, step):
l = arange(base - step, base - area, -step)
r = arange(base, base + area, step)
return hstack([l, r])
x_range = around(p[0], args.area, args.step)
y_range = around(p[1], args.area, args.step)
m = Marker()
m.header.frame_id = 'odom'
m.type = Marker.CUBE_LIST
m.action = Marker.ADD
class RobotGame():
def __init__(self):
rospy.init_node("robotGame")
self.joint_names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
self.his_joint_values = [
-2.5450077537365753e-08, 2.544688075917041e-08,
2.3532384855862176e-05, 7.785478886557229e-05,
-2.0168301624323703e-06, -8.301148533007563e-07,
-2.2624831839124226e-06
]
self.my_joint_values = self.his_joint_values
self.arm_joint_upper_limits = np.array(
[2.8973, 1.7628, 2.8973, 0.0175, 2.8973, 3.7525, 2.8973])
self.arm_joint_bottom_limits = np.array(
[-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973])
self.arm_joint_diff = self.arm_joint_upper_limits - self.arm_joint_bottom_limits
#OPTION 1:
self.observation_bound = np.array([100] * 13)
self.action_bound2 = np.array([0.1] * 7)
self.action_space = spaces.Box(low=-self.action_bound2,
high=self.action_bound2)
self.observation_space = spaces.Box(low=-self.observation_bound,
high=self.observation_bound)
#OPTION 2:
# self.observation_bound = np.array([5]*6)
# self.action_bound = np.array([0.05]*3)
# self.action_space = spaces.Box(low=-self.action_bound, high=self.action_bound)
# self.observation_space = spaces.Box(low=-self.observation_bound, high=self.observation_bound )
self.action_bound = [(-self.action_bound2).tolist(),
self.action_bound2.tolist()]
self.action_dim = self.action_space.shape[0]
self.state_dim = self.observation_space.shape[0]
self.gamma = 1
self.one_joint_execution_duration = 0.01
self.execution_time = [(x + 1) * self.one_joint_execution_duration
for x in range(7)]
self.dist_threshold = 0.1
self.vector_from_center = np.array([0, 0, 0.05])
self.object_moved_threshold = 0.05
self.tf = TransformListener()
self.sub = rospy.Subscriber('/joint_states', JointState, self.jsCB)
self.box = Box()
self.manipulator = Manipulator()
#self.reset()
def jsCB(self, msg):
temp_dict = dict(zip(msg.name, msg.position))
self.his_joint_values = [temp_dict[x] for x in self.joint_names]
def isInMotion(self):
for k in range(7):
val1 = self.my_joint_values[k]
val2 = self.his_joint_values[k]
if val1 - val2 > 1.0e-1 or val1 - val2 < -1.0e-1:
return True
return False
def getGripperPose3(self, reference):
self.tf.waitForTransform(reference, "/panda_leftfinger", rospy.Time(),
rospy.Duration(10))
t = self.tf.getLatestCommonTime(reference, "/panda_leftfinger")
position, quaternion = self.tf.lookupTransform(reference,
"/panda_leftfinger", t)
return np.array([position[0], position[1], position[2]])
def getGripperPose2(self):
pos = self.manipulator.arm.get_current_pose()
return pos #np.array([pos.position.x, pos.position.y, pos.position.z])
def getGripperPose(self, reference):
self.tf.waitForTransform(reference, "/panda_hand", rospy.Time(),
rospy.Duration(10))
t = self.tf.getLatestCommonTime(reference, "/panda_hand")
position, quaternion = self.tf.lookupTransform(reference,
"/panda_hand", t)
return np.array([position[0], position[1], position[2]])
def getRequiredJoints(self, delta):
pos = self.manipulator.arm.get_current_pose()
#gripperPos = self.getGripperPose('/panda_link0')
pos.position.x += delta[0]
pos.position.y += delta[1]
pos.position.z += delta[2]
#pos.orientation = None
print "IK SOLUTION OF: ", pos
return self.manipulator.arm.get_IK(pos)
def getDist(self):
currentPos = self.getGripperPose('/world')
pos = self.box.get_position()
self.destPos = np.array(
[pos.position.x, pos.position.y, pos.position.z]) #0.05??
#TODO: CHECK!!!
#print "box pos (GAZEBO INTERFACE): ", self.destPos
# self.tf.waitForTransform('/world',"box",rospy.Time(),rospy.Duration(10))
# t = self.tf.getLatestCommonTime('/world', "box")
# position, quaternion = self.tf.lookupTransform('/world',"box",t)
# print "box pos (TF): ", position
return np.linalg.norm(currentPos - self.destPos -
self.vector_from_center)
def reset(self):
self.my_joint_values = self.random_joint_initialize()
self.manipulator.arm.execute_trajectory(
[self.my_joint_values], [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
self.waitTillReady2()
pos = Pose()
pos.position.x = uniform(0.15, 0.4)
pos.position.y = uniform(-0.3, 0)
pos.position.z = 0.8
self.box.set_position(pos)
self.box.place_on_table()
rospy.sleep(1)
pos = self.box.get_position()
self.startDestPos = np.array(
[pos.position.x, pos.position.y, pos.position.z])
self.destPos = np.array(
[pos.position.x, pos.position.y, pos.position.z])
#OPTION 1:
return np.concatenate(
(self.my_joint_values, self.getGripperPose('world'), self.destPos))
#OPTION 2:
#return np.concatenate((self.getGripperPose2(), self.destPos))
def random_joint_initialize(self):
return np.random.uniform(self.arm_joint_bottom_limits,
self.arm_joint_upper_limits, 7).tolist()
def waitTillReady1(self):
state = self.manipulator.arm.client.get_state()
print state
while not state == GoalStatus.SUCCEEDED:
rospy.sleep(0.01)
state = self.manipulator.arm.client.get_state()
if state == GoalStatus.ABORTED:
self.manipulator.arm.execute_trajectory([self.my_joint_values],
self.execution_time)
rospy.sleep(0.01)
state = self.manipulator.arm.client.get_state()
print state
def waitTillReady2(self):
for k in range(300):
if not self.isInMotion(): return
rospy.sleep(0.01)
print "SOMETHING IS WRONG, STOPPED WAITING AFTER 3 SECS!"
# while self.isInMotion():
# rospy.sleep(0.01)
def waitTillReady3(self):
while not self.manipulator.arm.client.wait_for_result():
rospy.sleep(0.01)
print self.manipulator.arm.client.wait_for_result()
def step(self, delta):
done = False
prevDist = self.getDist()
#OPTION 1:
joint_values = np.clip(self.my_joint_values + delta,
self.arm_joint_bottom_limits,
self.arm_joint_upper_limits)
#OPTION 2:
# joint_values = self.getRequiredJoints(delta)
# print "IS: ", joint_values
#OPTION 3:
#Just go to the object XD
# stamped_pos = PoseStamped()
# pos = self.box.get_position()
# pos.position.z += 0.2
# pos.orientation = self.getGripperPose2().orientation
# stamped_pos.pose = pos
# stamped_pos.header.frame_id = '/world'
# print stamped_pos
# pos = self.tf.transformPose("/panda_link0", stamped_pos)
# print pos
#joint_values = self.manipulator.arm.get_IK(pos.pose)
#print joint_values
# if joint_values is None:
# print "Gripper position is out of bounds, penalty -10"
# return np.concatenate((self.getGripperPose('/world'), self.destPos)), -10, done, None
self.my_joint_values = joint_values
self.manipulator.arm.execute_trajectory([self.my_joint_values],
self.execution_time)
#OPTION 3 - Continued
#Burayi sonra sil
# self.waitTillReady2()
# pos.pose.position.z -= 0.15
# joint_values = self.manipulator.arm.get_IK(pos.pose)
# self.my_joint_values = joint_values
# self.manipulator.arm.execute_trajectory([self.my_joint_values], self.execution_time)
self.waitTillReady2()
curDist = self.getDist()
reward = -(curDist * curDist + 0.5 * (prevDist - curDist) *
(prevDist - curDist))
#reward = prevDist - curDist
#print "CURRENT DISTANCE: ", curDist
#print "Dest pos: ", self.destPos
info = False
if curDist <= self.dist_threshold:
reward += 100
done = True
info = True
elif self.destPos[2] < 0.3:
print "OBJECT FELL OFF THE GROUND! PENALTY: -10"
reward -= 10
done = True
info = False
elif np.linalg.norm(self.destPos -
self.startDestPos) >= self.object_moved_threshold:
#print "OBJECT IS MOVED! PENALTY: -20"
reward -= 10 #TODO: give penalty instead of reward ?
done = True
info = False
#OPTION 1:
return np.concatenate(
(self.my_joint_values, self.getGripperPose('world'),
self.destPos)), reward, done, info
#OOTION 2:
#return np.concatenate((self.getGripperPose2(), self.destPos)), reward, done, None
def done(self):
self.sub.unregister()
rospy.signal_shutdown("done")
