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 ForceFromGravity(object):
def __init__(self):
self.tf_l = TransformListener()
self.last_wrench = None
self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
WrenchStamped,
self.wrench_cb,
queue_size=1)
def wrench_cb(self, data):
self.last_wrench = data
def compute_forces(self):
qs = self.get_orientation()
o = qs.quaternion
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("wrist_left_ft_link rpy vs world: " + str( (round(r, 3), round(p, 3), round(y, 3)) ))
rospy.loginfo("in degrees: " + str( (round(degrees(r), 3), round(degrees(p), 3), round(degrees(y), 3)) ))
hand_total_force = 10 # 10 Newton, near to a Kg
fx = hand_total_force * cos(r) * -1.0 # hack
fy = hand_total_force * cos(p)
fz = hand_total_force * cos(y)
#total = abs(fx) + abs(fy) + abs(fz)
#rospy.loginfo("fx, fy, fz, total:")
#rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
return fx, fy, fz
def get_last_forces(self):
f = self.last_wrench.wrench.force
return f.x, f.y, f.z
def get_orientation(self, from_frame="wrist_left_ft_link"):
qs = QuaternionStamped()
qs.quaternion.w = 1.0
qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
qs.header.frame_id = "/" + from_frame
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_q = self.tf_l.transformQuaternion("/world", qs)
transform_ok = True
return world_q
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
qs.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
cx, cy, cz = self.compute_forces()
c_total = abs(cx) + abs(cy) + abs(cz)
fx, fy, fz = self.get_last_forces()
f_total = abs(fx) + abs(fy) + abs(fz)
rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" + str( (round(cx, 3), round(cy, 3), round(cz, 3) )))
rospy.loginfo("Real Forces :" + str(round(f_total, 3)) + "\n" + str( (round(fx, 3), round(fy, 3), round(fz, 3) )))
r.sleep()
def listener_server(data):
"""Function that obtains the position of the robot once the service is called.
@param data: input of the service:
@param data.writePose: boolean whether or not to get the position
@param data.argument: string with reason to save the position"""
if data.writePose:
try:
tfmine = TransformListener()
now = rospy.Time()
tfmine.waitForTransform("/map", "/base_link", now,
rospy.Duration(1))
time = tfmine.getLatestCommonTime("/map", "/base_link")
posit, quaternion = tfmine.lookupTransform("/map", "/base_link",
time)
yaw = trans.euler_from_quaternion(quaternion)[2]
except Exception as e:
print "Error at lookup getting position:"
print e
return False
try:
line = str(posit[0]) + "," + str(
posit[1]) + "," + str(yaw) + "," + str(data.argument) + "\n"
file = open("waypoints.txt", 'a')
file.write(line)
return True
except Exception as e:
print "Error writing position to file:"
print e
return False
else:
return True
class myNode:
def __init__(self, *args):
self.tf = TransformListener()
# rospy.Subscriber(...)
# ...
def some_method(self):
if self.tf.frameExists(
"left_fingertip_sensor_s0") and self.tf.frameExists("world"):
t = self.tf.getLatestCommonTime("left_fingertip_sensor_s0",
"world")
position, quaternion = self.tf.lookupTransform(
"left_fingertip_sensor_s0", "world", t)
print position, quaternion
else:
print "not found frame"
def fury(self):
# now = rospy.Time.now()
self.tf.waitForTransform("pbase_link", "world", rospy.Time(0),
rospy.Duration(10.0))
print "waited"
(trans, rot) = self.tf.lookupTransform("left_fingertip_sensor_s0",
"world", rospy.Time(0))
print trans, rot
return trans
def locations(self, frame1, frame2):
self.tf.waitForTransform(frame1, frame2, rospy.Time(0),
rospy.Duration(10.0))
print "waited"
(trans, rot) = self.tf.lookupTransform(frame1, frame2, rospy.Time(0))
print trans, rot
class FTSCalibSampler:
def __init__(self, filename='poses.txt'):
print('init')
self.tf = TransformListener()
self.base_link = 'base_link'
self.tool_link = 'fts_toolside'
self.pose_cnt = 0
self.file = open(filename, 'w+')
try:
self.tf.waitForTransform(self.base_link, self.tool_link,
rospy.Time(), rospy.Duration(5.0))
except tf.Exception: # likely a timeout
print(
"Timeout while waiting for the TF transformation with the map!"
" Is someone publishing TF tansforms?\n ROS positions won't be available."
)
self.tf_running = False
def update(self):
if self.tf.frameExists(self.base_link) and self.tf.frameExists(
self.tool_link):
t = self.tf.getLatestCommonTime(self.base_link, self.tool_link)
position, quaternion = self.tf.lookupTransform(
self.base_link, self.tool_link, t)
self.new_pose = position + list(euler_from_quaternion(quaternion))
print(self.new_pose)
def write(self):
self.file.write('pose{}: {}\n'.format(self.pose_cnt,
str(self.new_pose)))
self.pose_cnt += 1
def transform_cloud_to_map(cloud):
rospy.loginfo("to map from " + cloud.header.frame_id)
transformation_store = TransformListener()
rospy.sleep(2)
t = transformation_store.getLatestCommonTime("map", cloud.header.frame_id)
tr_r = transformation_store.lookupTransform("map", cloud.header.frame_id,
t)
tr = Transform()
tr.translation = Vector3(tr_r[0][0], tr_r[0][1], tr_r[0][2])
tr.rotation = Quaternion(tr_r[1][0], tr_r[1][1], tr_r[1][2], tr_r[1][3])
tr_s = TransformStamped()
tr_s.header = std_msgs.msg.Header()
tr_s.header.stamp = rospy.Time.now()
tr_s.header.frame_id = "map"
tr_s.child_frame_id = cloud.header.frame_id
tr_s.transform = tr
t_kdl = transform_to_kdl(tr_s)
points_out = []
for p_in in pc2.read_points(cloud, field_names=["x", "y", "z", "rgb"]):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append([p_out[0], p_out[1], p_out[2]])
cloud.header.frame_id = "map"
res = pc2.create_cloud(cloud.header, cloud.fields, points_out)
rospy.loginfo(cloud.header)
return res
def __init__(self, target_link):
rospy.init_node('GetJointPose')
tf_listener = TransformListener()
base_link = "base_link"
print "waiting for transform"
tf_listener.waitForTransform (target_link, base_link, rospy.Time(), rospy.Duration(4.0))
print "done waiting"
if not tf_listener.frameExists("base_link"):
print "ERROR NO FRAME base_link"
return
elif not tf_listener.frameExists(target_link):
print "ERROR NO FRAME" + target_link
return
else:
t = tf_listener.getLatestCommonTime("/base_link", target_link)
joint_pose = geometry_msgs.msg.PoseStamped()
joint_pose.header.frame_id = target_link
joint_pose.pose.orientation.w = 1.0 # Neutral orientation
pose_from_base = tf_listener.transformPose("/base_link", joint_pose)
print "Position from " + base_link + " to " + target_link
print pose_from_base
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 Leader():
def __init__(self, goals):
rospy.init_node('leader', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.leaderFrame = rospy.get_param("~leaderFrame", "/leader")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# self.leaderAdvertise=rospy.Publisher('leaderPosition',PoseStamped,queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
self.goals = goals
self.takeoffFlag = 0
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def cmdVelCallback(self, data):
if data.linear.z != 0.0 and self.takeoffFlag == 0:
self.takeoffFlag = 1
rospy.sleep(10)
self.takeoffFlag = 2
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
while not rospy.is_shutdown():
self.calc_goal(goal, self.goalIndex)
self.pubGoal.publish(goal)
# self.leaderAdvertise.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 self.takeoffFlag == 1:
self.goalIndex = 0
elif self.takeoffFlag == 2 and self.goalIndex < len(
self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4] * 2)
rospy.loginfo(self.goalIndex)
self.goalIndex += 1
def calc_goal(self, goal, index):
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[index][0]
goal.pose.position.y = self.goals[index][1]
goal.pose.position.z = self.goals[index][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[index][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]
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 LaunchObserver(object):
"""
Keeps track of the flying/landed state of a single drone, and publishes
a tf message keeping track of the coordinates from which the drone most recently launched.
"""
def __init__(self):
self.tfl = TransformListener()
self.tfb = TransformBroadcaster()
self.flying_state_sub = rospy.Subscriber(
'states/ardrone3/PilotingState/FlyingStateChanged',
Ardrone3PilotingStateFlyingStateChanged, self.on_flying_state)
self.is_flying = True
self.RATE = 5 # republish hz
self.saved_translation = [0, 0, 0] # In meters
self.saved_yaw = 0 # In radians
self.name = rospy.get_namespace().strip('/')
self.base_link = self.name + '/base_link'
self.launch = self.name + '/launch'
self.odom = self.name + '/odom'
def on_flying_state(self, msg):
self.is_flying = msg.state in [
Ardrone3PilotingStateFlyingStateChanged.state_takingoff,
Ardrone3PilotingStateFlyingStateChanged.state_hovering,
Ardrone3PilotingStateFlyingStateChanged.state_flying,
Ardrone3PilotingStateFlyingStateChanged.state_landing,
Ardrone3PilotingStateFlyingStateChanged.state_emergency_landing
]
def spin(self):
r = rospy.Rate(self.RATE)
self.tfl.waitForTransform(self.odom, self.base_link, rospy.Time(0),
rospy.Duration.from_sec(99999))
while not rospy.is_shutdown():
if not self.is_flying:
# On the ground, update the transform
pos, quat = self.tfl.lookupTransform(self.base_link, self.odom,
rospy.Time(0))
pos[2] = 0
self.saved_translation = pos
_, _, self.saved_yaw = euler_from_quaternion(quat)
time = max(rospy.Time.now(),
self.tfl.getLatestCommonTime(
self.odom, self.base_link)) + (
2 * rospy.Duration.from_sec(1.0 / self.RATE))
self.tfb.sendTransform(self.saved_translation,
quaternion_from_euler(0, 0, self.saved_yaw),
time, self.odom, self.launch)
r.sleep()
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 Follower():
def __init__(self, leaderFrame, radius=0.5, phase=0, pointNum=2000):
rospy.init_node('follower', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB)
self.listener = TransformListener()
self.goal = PoseStamped()
self.leaderFrame = leaderFrame
self.radius = radius
self.phase = 0
self.pointNum = 2000
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
self.goal = PoseStamped()
self.goal.header.seq = 0
self.goal.header.frame_id = self.worldFrame
self.goal.pose.orientation.w = 1
while not rospy.is_shutdown():
self.pubGoal.publish(self.goal)
# rospy.loginfo(self.goal)
# rospy.loginfo(self.worldFrame)
# rospy.loginfo(self.leaderFrame)
# rospy.loginfo(self.frame)
t = self.listener.getLatestCommonTime(self.worldFrame,
self.leaderFrame)
if self.listener.canTransform(self.worldFrame, self.leaderFrame,
t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.leaderFrame, t)
# rospy.loginfo(position)
# rospy.loginfo(quaternion)
self.followerGoalGenerate(position, quaternion)
rospy.sleep(0.02)
def followerGoalGenerate(self, position, quaternion):
# rospy.loginfo("info received!")
angle = self.goalIndex / self.pointNum * 2 * math.pi + self.phase
self.goal.header.seq += 1
self.goal.header.stamp = rospy.Time.now()
self.goal.pose.position.x = position[0] + self.radius * math.cos(angle)
self.goal.pose.position.y = position[1] + self.radius * math.sin(angle)
# self.goal.pose.position.z=position.z
self.goal.pose.position.z = 0.8
self.goal.pose.orientation.w = quaternion[3]
self.goal.pose.orientation.x = quaternion[0]
self.goal.pose.orientation.y = quaternion[1]
self.goal.pose.orientation.z = quaternion[2]
self.goalIndex = self.goalIndex + 1
class Normal():
def __init__(self):
rospy.init_node('follower', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
# rospy.Subscriber("/"+leaderName+'/leaderPosition',PoseStamped,self.followerSubCB)
self.listener = TransformListener()
self.goal=PoseStamped()
# 第一件事情，跟随这个目标，这个目标的格式应该是一个frame
self.leaderFrame=rospy.get_param("~leaderFrame","")
self.offsetX=rospy.get_param("~offsetX","0")
self.offsetY=rospy.get_param("~offsetY","0")
# self.offsetZ=rospy.get_param("~offsetZ","0")
# 第二件事情，广播自身的位置吧
# self.pubAttitude=rospy.Publisher('pose',)
# 第三件事情，侦听manager节点的状态信息
self.takeoffFlag = 0
self.goalIndex = 0
rospy.loginfo("demo start!!!!!!!")
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0))
self.goal = PoseStamped()
self.goal.header.seq = 0
self.goal.header.frame_id = self.worldFrame
self.goal.pose.orientation.w=1
while not rospy.is_shutdown():
self.pubGoal.publish(self.goal)
# rospy.loginfo(self.goal)
# rospy.loginfo(self.worldFrame)
# rospy.loginfo(self.leaderFrame)
# rospy.loginfo(self.frame)
t = self.listener.getLatestCommonTime(self.worldFrame, self.leaderFrame)
if self.listener.canTransform(self.worldFrame, self.leaderFrame, t):
position, quaternion = self.listener.lookupTransform(self.worldFrame, self.leaderFrame, t)
# rospy.loginfo(position)
# rospy.loginfo(quaternion)
self.followerGoalGenerate(position,quaternion)
rospy.sleep(0.02)
def followerGoalGenerate(self,position,quaternion):
# rospy.loginfo("info received!")
self.goal.header.seq += 1
self.goal.header.stamp = rospy.Time.now()
self.goal.pose.position.x=position[0]+float(self.offsetX)
self.goal.pose.position.y=position[1]+float(self.offsetY)
# self.goal.pose.position.z=position.z
self.goal.pose.position.z=0.8
self.goal.pose.orientation.w=quaternion[3]
self.goal.pose.orientation.x=quaternion[0]
self.goal.pose.orientation.y=quaternion[1]
self.goal.pose.orientation.z=quaternion[2]
class FaceInteractionDemo(object):
def __init__(self):
self.move_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
self.lock = RLock()
self.head = Head()
self.expressions = Expressions()
self.tf = TransformListener()
self.lastFacePos = None
rospy.sleep(0.5)
# Gets an equivalent point in the reference frame "destFrame"
def getPointStampedInFrame(self, pointStamped, destFrame):
# Wait until we can transform the pointStamped to destFrame
self.tf.waitForTransform(pointStamped.header.frame_id, destFrame, rospy.Time(), rospy.Duration(4.0))
# Set point header to the last common frame time between its frame and destFrame
pointStamped.header.stamp = self.tf.getLatestCommonTime(pointStamped.header.frame_id, destFrame)
# Transform the point to destFrame
return self.tf.transformPoint(destFrame, pointStamped)
def onFacePos(self, posStamped):
self.lastFacePos = posStamped
def navigateTo(self):
if (self.lastFacePos is not None and self.lock.acquire(blocking=False)):
self.expressions.nod_head()
pointStamped = self.getPointStampedInFrame(pointStamped, "base_link")
distance_to_base = sqrt(pointStamped.point.x ** 2 + pointStamped.point.y ** 2 + pointStamped.point.z ** 2)
unit_vector = { "x": pointStamped.point.x / distance_to_base,
"y": pointStamped.point.y / distance_to_base }
pointStamped.point.x = unit_vector["x"] * (distance_to_base - 0.5)
pointStamped.point.y = unit_vector["y"] * (distance_to_base - 0.5)
quaternion = Quaternion()
quaternion.w = 1
pose = Pose()
pose.position = pointStamped.point
pose.orientation = quaternion
poseStamped = PoseStamped()
poseStamped.header = pointStamped.header
pointStamped.pose = pose
self.move_pub.publish(poseStamped)
self.lock.release()
def onFaceCount(self, int8):
if (int8.data > 0):
self.expressions.be_happy()
else:
self.expressions.be_sad()
self.expressions.be_neutral()
class MocapPoseEstimator():
def __init__(self, node_name='mocap_pose_estimator'):
rospy.init_node(node_name)
self.tl = TransformListener()
self.robot_frame = rospy.get_namespace().strip('/') + '_mocap'
self.state_estimate_pub = rospy.Publisher('state_estimate',
Odometry,
queue_size=1)
self.last_state_estimate = None
self.pose_covariance = numpy.diag(
[0.001, 0.001, 0.001, 0.01, 0.01, 0.01]).reshape(-1)
self.twist_covariance = numpy.diag([0.01, 0.01, 0.01, 0.1, 0.1,
0.1]).reshape(-1)
def run(self):
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
stamp = self.tl.getLatestCommonTime('world', self.robot_frame)
pos, ori = self.tl.lookupTransform('world', self.robot_frame,
stamp)
except (LookupException, ConnectivityException,
ExtrapolationException, tfException):
continue
mocap_odom = Odometry(
Header(0, stamp, 'world'),
self.robot_frame,
PoseWithCovariance(Pose(Point(*pos), Quaternion(*ori)),
self.pose_covariance),
TwistWithCovariance(Twist(), self.twist_covariance),
)
if self.last_state_estimate is not None:
dt = (stamp - self.last_state_estimate.header.stamp).to_sec()
if dt > 0.0:
pose_0 = self.last_state_estimate.pose.pose
P0, O0, P1, O1 = [
msg_to_numpy(msg) for msg in
[pose_0.position, pose_0.orientation, pos, ori]
]
v_lin = (P1 - P0) / dt
v_ang = quats_to_twist(O0, O1) / dt
mocap_odom.twist.twist = Twist(Vector3(*v_lin),
Vector3(*v_ang))
self.state_estimate_pub.publish(mocap_odom)
self.last_state_estimate = mocap_odom
rate.sleep()
def run():
tt = tf.Transformer(True, rospy.Duration(10.0))
tfl = TransformListener()
#print tf.allFramesAsString()
print tfl.getFrameStrings()
if tfl.frameExists("/base_link") and tfl.frameExists("/base_footprint"):
t = tfl.getLatestCommonTime("/base_link", "/base_footprint")
position, quaternion = tfl.lookupTransform("/base_link",
"/base_footprint", t)
print position, quaternion
else:
print "no"
class Demo():
def __init__(self, goals):
rospy.init_node('demo', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher(
'goal', PoseStamped,
queue_size=1) #publish to topic goal with msg type PoseStamped
self.listener = TransformListener(
) #tflisterner is a method with functions relating to transforms
self.goals = goals #Assign nX5 matrix containing target waypoints
self.goalIndex = 0 # start with the first row of entries (first waypoint)
def run(self):
self.listener.waitForTransform(
self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)
) #Find the transform from world frame to body frame, returns bool on if it can find a transform
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
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(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 within position error bound, sleep and then move to next waypoint
if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.2 \
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.2 \
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.2 \
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 Demo():
def __init__(self, goals):
rospy.init_node('demo', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
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(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
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(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.goals[self.goalIndex][0]) < 0.1 \
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(8):
if self.goalIndex < len(self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
elif self.goalIndex == len(self.goals) - 1:
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex = 1
class Swarm():
def __init__(self):
rospy.init_node('demo', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 1
self.index = 1
with open('test.csv','rb') as myfile:
reader=csv.reader(myfile)
lines = [line for line in reader]
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = int(lines[self.goalIndex][3*index-1])
goal.pose.position.y = int(lines[self.goalIndex][3*index+0])
goal.pose.position.z = int(lines[self.goalIndex][3*index+1])
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
goal.pose.orientation.x = 0
goal.pose.orientation.y = 0
goal.pose.orientation.z = 0
goal.pose.orientation.w = 1
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] - int(lines[self.goalIndex][3*index-1])) < 0.25 \
and math.fabs(position[1] - int(lines[self.goalIndex][3*index+0])) < 0.25 \
and math.fabs(position[2] - int(lines[self.goalIndex][3*index+1])) < 0.25 \
and self.goalIndex < len(lines):
rospy.sleep(lines[self.goalIndex][1])
self.goalIndex += 1
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 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 TfNode:
def __init__(self, *args):
self.tf = TransformListener()
self.sub = rospy.Subscriber(
"/detectedObjs_primitive",
thesis_visualization.msg.objectLocalization,
self.__subCb,
queue_size=1)
self.msg = thesis_visualization.msg.objectLocalization()
self.thread_sub = Thread(target=self.start_sub)
def __subCb(self, msg):
self.msg = msg
print self.msg.modelList
def start_sub(self):
rospy.spin()
def subscribe(self):
self.thread_sub.start()
def unregister_sub(self):
self.sub.unregister()
rospy.signal_shutdown("close subcriber")
def example_function(self):
if self.tf.frameExists("/world") and self.tf.frameExists(
"/kinect2_depth"):
t = self.tf.getLatestCommonTime("/world", "/kinect2_depth")
p1 = geometry_msgs.msg.PoseStamped()
p1.header.frame_id = "kinect2_depth"
p1.pose.orientation.w = 1.0 # Neutral orientation
p_in_base = self.tf.transformPose("/world", p1_)
print "Position of the fingertip in the robot base:"
print p_in_base
def pcd2worldFrame(self, pcd):
self.tf.transformPointCloud("world", pcd)
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()
# Code for moving the robots joints
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'
]
# 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()
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.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]
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.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if (joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
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 move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(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)
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]))
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
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_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)
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 Follow():
def __init__(self, goals):
rospy.init_node('follow', anonymous=True)
self.worldFrame = rospy.get_param("~worldFrame", "/world")
self.frame = rospy.get_param("~frame")
self.goal = rospy.get_param("~goal")
self.x = rospy.get_param("~x")
self.y = rospy.get_param("~y")
self.z = rospy.get_param("~z")
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))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
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, 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)
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)
#rospy.loginfo(rpy)
if math.fabs(position[0] - self.x) < 0.25 \
and math.fabs(position[1] - self.y) < 0.25 \
and math.fabs(position[2] - self.z) < 0.25 \
and math.fabs(rpy[2] - 0) < math.radians(10):
rospy.sleep(3)
self.goalIndex += 1
break
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
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]
t = self.listener.getLatestCommonTime(self.worldFrame, self.goal)
if self.listener.canTransform(self.worldFrame, self.goal, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.goal, t)
goal.pose.position.x = position[0] - 0.8 * math.sin(rpy[2])
goal.pose.position.y = position[1] + 0.8 * math.cos(rpy[2])
goal.pose.position.z = position[2] + 0.5
rpy = tf.transformations.euler_from_quaternion(quaternion)
#rospy.loginfo(rpy)
self.pubGoal.publish(goal)
'(see description, default: 0.3)', default=0.3,
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, queue_size = 50)
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
class pose_detector:
# visualize
colors = [[255, 0, 0], [255, 85, 0], [255, 170, 0], [255, 255, 0], [170, 255, 0], [85, 255, 0], [0, 255, 0], \
[0, 255, 85], [0, 255, 170], [0, 255, 255], [0, 170, 255], [0, 85, 255], [0, 0, 255], [85, 0, 255], \
[170, 0, 255], [255, 0, 255], [255, 0, 170], [255, 0, 85]]
model = pose_model(models)
model.load_state_dict(torch.load(weight_name))
model.cuda()
model.float()
model.eval()
param_, model_ = config_reader()
def __init__(self, params):
self.transform = TransformListener()
self.transformer = Transformer(True, rospy.Duration(10.0))
self.params = params
self.cvbridge = CvBridge()
self.OBJ_TOPIC = self.params['obj_topic']
self.POSE_TOPIC = self.params['pose_topic']
self.sub_obj = rospy.Subscriber(self.OBJ_TOPIC,
objs_array,
self.callback_objs,
queue_size=1)
self.pose_pub = rospy.Publisher(self.POSE_TOPIC,
objs_array,
queue_size=1)
self.flag = 0
self.last_detect = time.time()
return None
def callback_act(self, msg):
if msg.data == 1: self.flag = 1
def callback_objs(self, msg):
#if self.flag == 0 : return None
tic = time.time()
humans = []
human_imgs = []
scales = []
pubmsg = msg
for item in msg.objects:
if item.class_string == 'person':
item.joints = [-1] * 36
humans.append(item)
img = self.cvbridge.imgmsg_to_cv2(item.cropped, 'bgr8')
imgg = np.zeros(
(self.model_['boxsize'], self.model_['boxsize'], 3))
if img.shape[0] >= img.shape[1]:
scale = float(self.model_['boxsize']) / img.shape[0]
else:
scale = float(self.model_['boxsize']) / img.shape[1]
scales.append(scale)
img2 = cv2.resize(img, (0, 0),
fx=scale,
fy=scale,
interpolation=cv2.INTER_CUBIC)
imgg[:img2.shape[0], :img2.shape[1]] = img2.copy()
human_imgs.append(imgg)
if len(humans) == 0:
self.pose_pub.publish(msg)
return None
minibatch = np.stack(human_imgs, axis=0)
num_run = minibatch.shape[0] // 20
humans_updated = []
for i in range(num_run + 1):
s = i * 20
e = min(minibatch.shape[0], (i + 1) * 20)
output = self.detect(minibatch[s:e], humans[s:e], scales[s:e])
humans_updated += output
for i in range(len(humans_updated)):
humans_updated[i] = self.post_processing(humans_updated[i])
pubmsg.objects = humans_updated
self.pose_pub.publish(pubmsg)
toc = time.time()
print '[DIP] Pose estimation. Elapsed time : ', toc - tic
self.last_detect = time.time()
self.flag = 0
def get_loc(
self,
p=np.array([0, 0, 0]),
o=np.array([0, 0, 0, 1]),
source='CameraTop_frame',
target='map'): #pose = np.array([x,y,z]) : position w.r.t. robot
pp = PoseStamped()
pp.pose.position.x = p[0]
pp.pose.position.y = p[1]
pp.pose.position.z = p[2]
pp.pose.orientation.x = o[0]
pp.pose.orientation.y = o[1]
pp.pose.orientation.z = o[2]
pp.pose.orientation.w = o[3]
#pp.header.stamp = rospy.get_rostime()
pp.header.frame_id = source #'CameraDepth_frame'
#print rospy.Time()
self.transform.waitForTransform(target,
source,
time=rospy.Time(),
timeout=rospy.Duration(3.0))
asdf = self.transform.getLatestCommonTime(target, source)
pp.header.stamp = asdf
result = self.transform.transformPose(target, pp)
result_p = np.array([
result.pose.position.x, result.pose.position.y,
result.pose.position.z
])
result_o = np.array([
result.pose.orientation.x, result.pose.orientation.y,
result.pose.orientation.z, result.pose.orientation.w
])
return result_p, result_o
def post_processing(self, h):
sitting_thr = -0.05
'''
0 1 nose
2 3 neck
4 5 r_shoulder
6 7 r_elbow
8 9 r_wrist
10 11 l_shoulder
12 13 l_elbow
14 15 l_wrist
16 17 r_pelvis
18 19 r_knee
20 21 r_anckle
22 23 l_pervis
24 25 l_knee
26 27 l_ankle
28 29 r_eye
30 31 l_eye
32 33 r_ear
34 35 l_ear
'''
#rasing hand
if h.joints[10] >= 0 and h.joints[12] >= 0 and h.joints[10] > h.joints[
12]:
h.tags.append('lwaving')
h.tags.append('waving')
if h.joints[4] >= 0 and h.joints[6] >= 0 and h.joints[4] > h.joints[6]:
h.tags.append('rwaving')
h.tags.append('waving')
#sitting
shoulder_h = -9999
cropped_cloud = self.cvbridge.imgmsg_to_cv2(
h.cropped_cloud, desired_encoding="passthrough")
if h.joints[4] > 0 and h.joints[5] > 0:
shoulder_h = max(
shoulder_h,
self.get_pos_wrt_robot(cropped_cloud, h.joints[4],
h.joints[5])[2])
if h.joints[10] > 0 and h.joints[11] > 0:
shoulder_h = max(
shoulder_h,
self.get_pos_wrt_robot(cropped_cloud, h.joints[10],
h.joints[11])[2])
if shoulder_h < sitting_thr and shoulder_h > -9999:
h.tags.append('sitting')
return h
def get_pos_wrt_robot(self,
cropped_cloud,
x,
y,
size=10,
scan_len=50,
scan='point'):
#scan : point(around), vertical(line)
h = cropped_cloud.shape[0]
w = cropped_cloud.shape[1]
if scan == 'point':
x1 = min(h, max(0, x - size // 2))
x2 = min(h, max(0, x + size // 2))
y1 = min(w, max(0, y - size // 2))
y2 = min(w, max(0, y + size // 2))
roi = cropped_cloud[x1:x2, y1:y2]
mask = roi[:, :, 0] > 0
masked = roi[mask]
if masked.size == 0: return np.array([0, 0, 0])
mask = masked[:, 0] == masked[:, 0].min()
masked = masked[mask]
return masked[0] #self.point_clouds[x,y]
else:
xx1 = min(h, max(0, x - scan_len))
xx2 = min(h, max(0, x + scan_len))
roi = cropped_cloud[xx1:xx2, y - 2:y + 2, :]
mask = roi[:, :, 0] > 0
masked = roi[mask]
if masked.size == 0: return np.array([0, 0, 0])
mask = masked[:, 0] == masked[:, 0].min()
masked = masked[mask]
return masked[0] #self.point_clouds[x,y]
def detect(self, minibatch, humans, scales):
ms = minibatch.shape
tic = time.time()
imageToTest = Variable(T.transpose(
T.transpose((torch.from_numpy(minibatch).float() / 256.0) - 0.5, 2,
3), 1, 2),
volatile=True).cuda()
output1, output2 = self.model(imageToTest)
heatmap_avg = nn.UpsamplingBilinear2d((ms[1], ms[2])).cuda()(output2)
paf_avg = nn.UpsamplingBilinear2d((ms[1], ms[2])).cuda()(output1)
heatmap_avg = T.transpose(heatmap_avg, 1, 2)
heatmap_avg = T.transpose(heatmap_avg, 2, 3)
heatmap_avg = heatmap_avg.cpu().data.numpy()
#print 'heatmap shape : ' , heatmap_avg.shape
paf_avg = paf_avg.cpu().data.numpy()
heatmap_avg = heatmap_avg[:, :, :, :-1]
map = gaussian_filter1d(heatmap_avg, sigma=3, axis=2)
map = gaussian_filter1d(map, sigma=3, axis=1)
map_left = np.zeros(map.shape)
map_left[:, 1:, :, :] = map[:, :-1, :, :]
map_right = np.zeros(map.shape)
map_right[:, :-1, :, :] = map[:, 1:, :, :]
map_up = np.zeros(map.shape)
map_up[:, :, 1:, :] = map[:, :, :-1, :]
map_down = np.zeros(map.shape)
map_down[:, :, :-1, :] = map[:, :, 1:, :]
peaks_binary = np.logical_and.reduce(
(map >= map_left, map >= map_right, map >= map_up, map >= map_down,
map > self.param_['thre1']))
peaks = np.nonzero(peaks_binary)
for i in range(peaks[0].shape[0]):
human_idx = int(peaks[0][i])
x = int(peaks[1][i] / scales[human_idx])
y = int(peaks[2][i] / scales[human_idx])
#x = peaks[1][i]
#y = peaks[2][i]
joint = peaks[3][i]
humans[human_idx].joints[joint * 2] = x
humans[human_idx].joints[joint * 2 + 1] = y
return humans
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 ArmByFtWrist(object):
def __init__(self):
rospy.loginfo("Initializing...")
# Node Hz rate
self.rate = 10
self.rate_changed = False
self.send_goals = False
# Limits
self.min_x = 0.0
self.max_x = 0.6
self.min_y = -0.5
self.max_y = -0.05
self.min_z = -0.2
self.max_z = 0.2
# Force stuff
self.fx_scaling = 0.0
self.fy_scaling = 0.0
self.fz_scaling = 0.0
self.fx_deadband = 0.0
self.fy_deadband = 0.0
self.fz_deadband = 0.0
# Torque stuff
self.tx_scaling = 0.0
self.ty_scaling = 0.0
self.tz_scaling = 0.0
self.tx_deadband = 0.0
self.ty_deadband = 0.0
self.tz_deadband = 0.0
self.axis_force = "zxy"
self.sign_force = "+++"
self.axis_torque = "zxy"
self.sign_torque = "+++"
# Signs adjusting fx, fy, fz, tx(roll), ty(pitch), tz(yaw)
# for the left hand of reemc right now
# And axis flipping
self.frame_fixer = WrenchFixer(1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
'z', 'x', 'y',
'z', 'x', 'y')
self.goal_frame_id = "arm_right_tool_link"
self.global_frame_id = "world"
self.wbc_goal_topic = "/whole_body_kinematic_controler/arm_right_tool_link_goal"
self.pose_to_follow_topic = "/pose_to_follow"
self.debug_topic = "/force_torqued_pose"
self.force_torque_topic = "/right_wrist_ft"
# All the previous params will be reset if there are parameters in the
# param server
self.dyn_rec_srv = Server(HandshakeConfig, self.dyn_rec_callback)
# Goal to send to WBC
# TODO: make this prettier
self.tf_l = TransformListener()
rospy.sleep(3.0) # Let the subscriber to its job
self.initial_pose = self.get_initial_pose()
self.tf_l = None
self.current_pose = self.initial_pose
self.last_pose_to_follow = deepcopy(self.current_pose)
if self.goal_frame_id[0] == '/':
self.goal_frame_id = self.goal_frame_id[1:]
self.pose_pub = rospy.Publisher(self.wbc_goal_topic,
PoseStamped,
queue_size=1)
# Safe debugging goal
self.debug_pose_pub = rospy.Publisher(self.debug_topic,
PoseStamped,
queue_size=1)
# Goal to follow
self.follow_sub = rospy.Subscriber(self.pose_to_follow_topic,
PoseStamped,
self.follow_pose_cb,
queue_size=1)
# Sensor input
self.last_wrench = None
self.wrench_sub = rospy.Subscriber(self.force_torque_topic,
WrenchStamped,
self.wrench_cb,
queue_size=1)
rospy.loginfo("Done initializing.")
def dyn_rec_callback(self, config, level):
"""
:param config:
:param level:
:return:
"""
rospy.loginfo("Received reconf call: " + str(config))
# Node Hz rate
if self.rate != config['rate']:
self.rate_changed = True
self.rate = config['rate']
self.send_goals = config['send_goals']
# Limits
self.min_x = config['min_x']
self.max_x = config['max_x']
self.min_y = config['min_y']
self.max_y = config['max_y']
self.min_z = config['min_z']
self.max_z = config['max_z']
# Force stuff
self.fx_scaling = config['fx_scaling']
self.fy_scaling = config['fy_scaling']
self.fz_scaling = config['fz_scaling']
self.fx_deadband = config['fx_deadband']
self.fy_deadband = config['fy_deadband']
self.fz_deadband = config['fz_deadband']
# Torque stuff
self.tx_scaling = config['tx_scaling']
self.ty_scaling = config['ty_scaling']
self.tz_scaling = config['tz_scaling']
self.tx_deadband = config['tx_deadband']
self.ty_deadband = config['ty_deadband']
self.tz_deadband = config['tz_deadband']
# Fixing transform stuff
self.axis_force = config['axis_force']
self.sign_force = config['sign_force']
self.axis_torque = config['axis_torque']
self.sign_torque = config['sign_torque']
if config['goal_frame_id'][0] == '/':
config['goal_frame_id'] = config['goal_frame_id'][1:]
if config['goal_frame_id'] != self.goal_frame_id:
self.goal_frame_id = config['goal_frame_id']
if config['global_frame_id'][0] == '/':
config['global_frame_id'] = config['global_frame_id'][1:]
if config['global_frame_id'] != self.global_frame_id:
self.global_frame_id = config['global_frame_id']
if self.wbc_goal_topic != config["wbc_goal_topic"]:
self.wbc_goal_topic = config["wbc_goal_topic"]
self.pose_pub = rospy.Publisher(self.wbc_goal_topic,
PoseStamped,
queue_size=1)
if self.debug_topic != config["debug_topic"]:
self.debug_topic = config["debug_topic"]
self.debug_pose_pub = rospy.Publisher(self.debug_topic,
PoseStamped,
queue_size=1)
if self.force_torque_topic != config["force_torque_topic"]:
self.force_torque_topic = config["force_torque_topic"]
self.wrench_sub = rospy.Subscriber(self.force_torque_topic,
WrenchStamped,
self.wrench_cb,
queue_size=1)
if self.pose_to_follow_topic != config["pose_to_follow_topic"]:
self.pose_to_follow_topic = config["pose_to_follow_topic"]
self.follow_sub = rospy.Subscriber(self.pose_to_follow_topic,
PoseStamped,
self.follow_pose_cb,
queue_size=1)
args = []
for sign in self.sign_force:
if sign == '+':
args.append(1.0)
else:
args.append(-1.0)
for sign in self.sign_torque:
if sign == '+':
args.append(1.0)
else:
args.append(-1.0)
args.extend([self.axis_force[0], self.axis_force[1], self.axis_force[2],
self.axis_torque[0], self.axis_torque[1], self.axis_torque[2]])
self.frame_fixer = WrenchFixer(*args)
return config
def follow_pose_cb(self, data):
if data.header.frame_id[0] == '/':
frame_id = data.header.frame_id[1:]
else:
frame_id = data.header.frame_id
if frame_id != self.global_frame_id:
rospy.logwarn(
"Poses to follow should be in frame " + self.global_frame_id +
" and is in " + str(frame_id) + ", transforming into " + self.global_frame_id + "...")
world_ps = self.transform_pose_to_world(
data.pose, from_frame=data.header.frame_id)
ps = PoseStamped()
ps.header.stamp = data.header.stamp
ps.header.frame_id = self.global_frame_id
ps.pose = world_ps
self.last_pose_to_follow = ps
else:
self.last_pose_to_follow = data
def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"):
ps = PoseStamped()
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
ps.header.frame_id = "/" + from_frame
# TODO: check pose being PoseStamped or Pose
ps.pose = pose
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_ps = self.tf_l.transformPose(self.global_frame_id, ps)
transform_ok = True
return world_ps
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
def wrench_cb(self, data):
self.last_wrench = data
def get_initial_pose(self):
zero_pose = Pose()
zero_pose.orientation.w = 1.0
current_pose = self.transform_pose_to_world(
zero_pose, from_frame=self.goal_frame_id)
return current_pose
def get_abs_total_force(self, wrench_msg):
f = wrench_msg.wrench.force
return abs(f.x) + abs(f.y) + abs(f.z)
def get_abs_total_torque(self, wrench_msg):
t = wrench_msg.wrench.torque
return abs(t.x) + abs(t.y) + abs(t.z)
def run(self):
rospy.loginfo(
"Waiting for first wrench message on: " + str(self.wrench_sub.resolved_name))
while not rospy.is_shutdown() and self.last_wrench is None:
rospy.sleep(0.2)
r = rospy.Rate(self.rate)
rospy.loginfo("Node running!")
while not rospy.is_shutdown():
# To change the node rate
if self.rate_changed:
r = rospy.Rate(self.rate)
self.rate_changed = False
self.follow_pose_with_admitance_by_ft()
r.sleep()
def follow_pose_with_admitance_by_ft(self):
fx, fy, fz = self.get_force_movement()
rospy.loginfo(
"fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3))))
ps = Pose()
if abs(fx) > self.fx_deadband:
ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx
if abs(fy) > self.fy_deadband:
ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy
if abs(fz) > self.fz_deadband:
ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz
tx, ty, tz = self.get_torque_movement()
rospy.loginfo(
"tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3))))
roll = pitch = yaw = 0.0
if abs(tx) > self.tx_deadband:
roll += (tx * self.tx_scaling) * self.frame_fixer.tx
if abs(ty) > self.ty_deadband:
pitch += (ty * self.ty_scaling) * self.frame_fixer.ty
if abs(tz) > self.tz_deadband:
yaw += (tz * self.tz_scaling) * self.frame_fixer.tz
q = quaternion_from_euler(roll, pitch, yaw)
ps.orientation = Quaternion(*q)
o = self.last_pose_to_follow.pose.orientation
r_lastp, p_lastp, y_lastp = euler_from_quaternion([o.x, o.y, o.z, o.w])
final_roll = r_lastp + roll
final_pitch = p_lastp + pitch
final_yaw = y_lastp + yaw
self.current_pose.pose.orientation = Quaternion(*quaternion_from_euler(final_roll,
final_pitch,
final_yaw))
self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + \
ps.position.x
self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + \
ps.position.y
self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + \
ps.position.z
self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x,
self.min_x,
self.max_x)
self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y,
self.min_y,
self.max_y)
self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z,
self.min_z,
self.max_z)
self.current_pose.header.stamp = rospy.Time.now()
if self.send_goals: # send MODIFIED GOALS
self.pose_pub.publish(self.current_pose)
else:
self.last_pose_to_follow.header.stamp = rospy.Time.now()
self.pose_pub.publish(self.last_pose_to_follow)
self.debug_pose_pub.publish(self.current_pose)
def get_force_movement(self):
f_x = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.x_axis)
f_y = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.y_axis)
f_z = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.z_axis)
return f_x, f_y, f_z
def get_torque_movement(self):
t_x = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.roll_axis)
t_y = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.pitch_axis)
t_z = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.yaw_axis)
return t_x, t_y, t_z
def sanitize(self, data, min_v, max_v):
if data > max_v:
return max_v
if data < min_v:
return min_v
return data
class Controller():
ActionTakeOff = 0
ActionHover = 1
ActionLand = 2
ActionAnimation = 3
def __init__(self):
self.lastNavdata = None
self.lastState = State.Unknown
rospy.on_shutdown(self.on_shutdown)
rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata)
self.pubTakeoff = rospy.Publisher('ardrone/takeoff',
Empty,
queue_size=1)
self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1)
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.setFlightAnimation = rospy.ServiceProxy(
'ardrone/setflightanimation', FlightAnim)
self.listener = TransformListener()
self.action = Controller.ActionTakeOff
self.pidX = PID(0.2, 0.12, 0.0, -0.3, 0.3, "x")
self.pidY = PID(0.2, 0.12, 0.0, -0.3, 0.3, "y")
self.pidZ = PID(1.0, 0, 0.0, -1.0, 1.0, "z")
self.pidYaw = PID(0.5, 0, 0.0, -0.6, 0.6, "yaw")
# X, Y, Z, Yaw
self.goals = [
[0.0, 0.0, 2.0, math.radians(0)],
"ANIM",
[0.0, 0.0, 2.0, math.radians(0)],
[1.0, 0.0, 1.5, math.radians(90)],
[1.0, 1.0, 0.8, math.radians(180)],
[-1.0, 1.0, 1.2, math.radians(0)],
[1.0, 0.0, 0.8, math.radians(0)],
]
self.goalIndex = 0
def on_navdata(self, data):
self.lastNavdata = data
if data.state != self.lastState:
rospy.loginfo("State Changed: " + str(data.state))
self.lastState = data.state
def on_shutdown(self):
rospy.loginfo("Shutdown: try to land...")
msg = Twist()
for i in range(0, 1000):
self.pubLand.publish()
self.pubNav.publish(msg)
rospy.sleep(1)
def run(self):
while not rospy.is_shutdown():
if self.action == Controller.ActionTakeOff:
if self.lastState == State.Landed:
self.pubTakeoff.publish()
elif self.lastState == State.Hovering or self.lastState == State.Flying or self.lastState == State.Flying2:
self.action = Controller.ActionHover
elif self.action == Controller.ActionLand:
msg = Twist()
self.pubNav.publish(msg)
self.pubLand.publish()
elif self.action == Controller.ActionHover:
rospy.loginfo('pid running')
# transform target world coordinates into local coordinates
targetWorld = PoseStamped()
t = self.listener.getLatestCommonTime(
"/vicon/ar_drone/ar_drone", "/world")
if self.listener.canTransform("/vicon/ar_drone/ar_drone",
"/world", t):
targetWorld.header.stamp = t
targetWorld.header.frame_id = "world"
targetWorld.pose.position.x = self.goals[self.goalIndex][0]
targetWorld.pose.position.y = self.goals[self.goalIndex][1]
targetWorld.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
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(
"/vicon/ar_drone/ar_drone", 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)
# Run PID controller and send navigation message
msg = Twist()
#msg.linear.x = self.pidX.update(velX, targetVelX)
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)
msg.angular.z = self.pidYaw.update(0.0, euler[2])
# disable hover mode
msg.angular.x = 1
self.pubNav.publish(msg)
if (math.fabs(targetDrone.pose.position.x) < 0.2
and math.fabs(targetDrone.pose.position.y) < 0.2
and math.fabs(targetDrone.pose.position.z) < 0.2
and math.fabs(euler[2]) < math.radians(20)):
if self.goalIndex < len(self.goals) - 1:
self.goalIndex += 1
if type(self.goals[self.goalIndex]) is str:
msg = Twist()
for i in range(0, 1000):
self.pubNav.publish(msg)
self.setFlightAnimation(8, 0)
rospy.sleep(1.0)
#for i in range(0, 1000):
# self.pubNav.publish(msg)
#rospy.sleep(1.5)
self.goalIndex += 1
rospy.loginfo("Next Goal (X,Y,Z,Yaw): " +
str(self.goals[self.goalIndex]))
else:
pass
#self.action = Controller.ActionLand
rospy.sleep(0.01)
class Rotate(smach.State):
#class handles the rotation until program is stopped
def __init__(self):
self.tf = TransformListener()
smach.State.__init__(self,
outcomes=['finished','failed'],
input_keys=['base_pose','stop_rotating','id'],
output_keys=['detected'])
rospy.Subscriber("/cob_people_detection/detection_tracker/face_position_array",DetectionArray, self.callback)
self.stop_rotating=False
self.detections= list()
self.false_detections=list()
def callback(self,msg):
# go through list of detections and append them to detection list
if len(msg.detections) >0:
#clear detection list
del self.detections[:]
for i in xrange( len(msg.detections)):
self.detections.append(msg.detections[i].label)
return
def execute(self, userdata):
sss.say(["I am going to take a look around now."])
# get position from tf
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)
# calculate angles from quaternion
[r,p,y]=euler_from_quaternion(quaternion)
#print r
#print p
#print y
#print position
else:
print "No transform available"
return "failed"
time.sleep(1)
self.stop_rotating=False
# create relative pose - x,y,theta
curr_pose=list()
curr_pose.append(0)
curr_pose.append(0)
curr_pose.append(0.1)
while not rospy.is_shutdown() and self.stop_rotating==False and curr_pose[2]< 3.14:
handle_base = sss.move_base_rel("base", curr_pose)
#check in detection and react appropriately
for det in self.detections:
# right person is detected
if det == userdata.id:
self.stop_rotating=True
sss.say(['I have found you, %s! Nice to see you.'%str(det)])
elif det in self.false_detections:
# false person is detected
print "Already in false detections"
# person detected is unknown - only react the first time
elif det == "Unknown":
print "Unknown face detected"
sss.say(['Hi! Nice to meet you, but I am still searching for %s.'%str(userdata.id)])
self.false_detections.append("Unknown")
# wrong face is detected the first time
else:
self.false_detections.append(det)
print "known - wrong face detected"
sss.say(['Hello %s! Have you seen %s.'%(str(det),str(userdata.id))])
#clear detection list, so it is not checked twice
del self.detections[:]
time.sleep(2)
print "-->stop rotating"
return 'finished'
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 KeyPointManager(object):
def __init__(self):
self.tf = TransformListener()
self.keyPointList = list()
def add(self, marker):
for i in range(len(self.keyPointList)):
if (self.keyPointList[i].id==marker.id):
return
position = self.transformMarkerToWorld(marker)
k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0., 0., 0., 1.))
self.keyPointList.append(k)
self.addWaypointMarker(k)
rospy.loginfo('Marker is added to following position')
rospy.loginfo(k.pose)
pass
def getWaypoints(self):
waypoints = list()
for i in range(len(self.keyPointList)):
waypoints.append(self.keyPointList[i].pose);
return waypoints
def keyPointListComplete(self):
if (len(self.keyPointList)==5):
self.keyPointList.sort(key=lambda x: x.id, reverse=True)
return True
return False
def markerHasValidId(self, marker):
if (marker.id>=61) and (marker.id<=65):
return True
return False
def transformMarkerToWorld(self, marker):
markerTag = "ar_marker_"+str(marker.id )
rospy.loginfo(markerTag);
if self.tf.frameExists("map") and self.tf.frameExists(markerTag):
self.tf.waitForTransform("map", markerTag, rospy.Time(), rospy.Duration(4.0))
t = self.tf.getLatestCommonTime("map", markerTag)
position, quaternion = self.tf.lookupTransform("map", markerTag, t)
return self.shiftPoint( position, quaternion)
def shiftPoint(self, position, quaternion):
try:
euler = euler_from_quaternion(quaternion)
yaw = euler[2]
tf_mat = np.array([[np.cos(yaw), -np.sin(yaw), 0, position[0]],[np.sin(yaw), np.cos(yaw), 0, position[1]],[0, 0, 1, 0],[0, 0, 0, 1]])
displacement = np.array([[1, 0, 0, 0],[0, 1, 0, 0.35],[0, 0, 1, 0],[0, 0, 0, 1]])
point_map = np.dot(tf_mat, displacement)
position = (point_map[0,3], point_map[1,3], 0)
except Exception as inst:
print type(inst) # the exception instance
print inst.args # arguments stored in .args
print inst
return position
def addWaypointMarker(self, keyPoint):
rospy.loginfo('Publishing marker')
# Set up our waypoint markers
marker_scale = 0.2
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = keyPoint.id
marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}
# Initialize the marker points list.
self.waypoint_markers = Marker()
self.waypoint_markers.ns = marker_ns
self.waypoint_markers.id = marker_id
self.waypoint_markers.type = Marker.CUBE_LIST
self.waypoint_markers.action = Marker.ADD
self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
self.waypoint_markers.scale.x = marker_scale
self.waypoint_markers.scale.y = marker_scale
self.waypoint_markers.color.r = marker_color['r']
self.waypoint_markers.color.g = marker_color['g']
self.waypoint_markers.color.b = marker_color['b']
self.waypoint_markers.color.a = marker_color['a']
self.waypoint_markers.header.frame_id = 'map'
self.waypoint_markers.header.stamp = rospy.Time.now()
self.waypoint_markers.points = list()
p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z)
self.waypoint_markers.points.append(p)
# Publish the waypoint markers
self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
self.marker_pub.publish(self.waypoint_markers)
class i2oNode(object):
def __init__(self, *args, **kwargs): #pylint: disable=unused-argument
rospy.init_node('i2oNode', anonymous=True)
self.tf = TransformListener()
# print str(dir(self.tf))
self.imu_to_baselink = None
# PUB/SUB Setup
self.pub = rospy.Publisher('imu2odom', Odometry, queue_size=10)
self.init_sub = rospy.Subscriber('initialize_localization', Bool, queue_size=10)
self.sub = rospy.Subscriber('imu', Imu, self.imu_callback)
# state model
self.baselink_model = State2D(0,0,0)
self.imu_model = State2D(0,0,0)
self.imu_zero = State2D(0,0,0)
self.init_state = False
# MAIN FUNCTION
def listal(self): # listen-talk
# run the node
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rate.sleep()
return 0
# === TRANSITION FUNCTION ===
def update_state_imu(self, state, imu_msg):
# update state with new imu_information
# Renamed state vars
# (time - time) => duration
dt = (imu_msg.header.stamp - state.timestamp()).to_sec()
final_orientation = self.quat_to_euler(imu_msg.orientation)
# jerk
twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt)
# intermediate vars
avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt)
ang_acc = self.ang_acc(twerk, state.alpha(), dt)
# lin_vel((a_measured, v0, dt))
lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt)
# assign final state
state.stamp = imu_msg.header.timestamp
state.point = self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt) # includes x, y
state.heading = final_orientation # includes heading
state.ang_acc = ang_acc # includes alpha
state.ang_vel = Vector.from_Vector3(imu_msg.angular_velocity) # includes omega
state.lin_vel = lin_vel # includes v
state.lin_acc = Vector.from_Vector3(imu_msg.linear_acceleration) # includes a
return state
def update_state_init(self, state, imu_msg):
# average state with new imu_information
# Renamed state vars
# (time - time) => duration
dt = (imu_msg.header.stamp - state.timestamp()).to_sec()
final_orientation = self.quat_to_euler(imu_msg.orientation)
# jerk
twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt)
# intermediate vars
avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt)
ang_acc = self.ang_acc(twerk, state.alpha(), dt)
# lin_vel((a_measured, v0, dt))
lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt)
# average final state
# so the values collected during the init are a moving average
# this all should be pretty stable around 0, because the robot should be stopped
# also, all state models should use this to zero-out their models
state.point = Vector.average(state.point, self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt)) # includes x, y
state.heading = state.heading/2 + final_orientation/2 # includes heading
state.ang_acc = Vector.average(state.ang_acc, ang_acc) # includes alpha
state.ang_vel = Vector.average(state.ang_vel, Vector.from_Vector3(imu_msg.angular_velocity)) # includes omega
state.lin_vel = Vector.average(state.lin_vel, lin_vel) # includes v
state.lin_acc = Vector.average(state.lin_acc, Vector.from_Vector3(imu_msg.linear_acceleration)) # includes a
return state
# DATA CALLBACK
def imu_callback(self, imu_msg):
# TODO(buckbaskin): implement initialization/zeroing, not necessarily here
if self.init_state:
# do initializations
self.imu_zero = self.update_state_init(self.imu_zero, imu_msg)
else:
# lookup Transformation here because of unknown imu frame until callback
if self.tf.frameExists('/base_link') and self.tf.frameExists(imu_msg.header.frame_id): #pylint: disable=no-member
try:
t = self.tf.getLatestCommonTime('/baselink', imu_msg.header.frame_id) #pylint: disable=no-member
position, quaternion = self.tf.lookupTransform('/baselink', imu_msg.header.frame_id, t) #pylint: disable=no-member
self.imu_to_baselink = [position, quaternion]
rospy.loginfo(' >>> Transform Found')
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo(' <<< Error finding transform')
# update state models
if self.imu_to_baselink and len(self.imu_to_baselink) == 2:
# the imu first updates the statemodel for the imu frame
self.imu_model = self.update_state_imu(self.imu_model, imu_msg)
# baselink_imu is imu data in the baselink frame
baselink_imu = self.convert_to_baselink(imu_msg, self.imu_model, self.imu_to_baselink)
# then the baselink (robot) model is updated using the transformed data
self.baselink_model = self.update_state_imu(self.baselink_model, baselink_imu)
# immediately publish data
self.pub.publish(self.baselink_model.to_msg())
else:
rospy.loginfo(' <<< Imu data not used. Saved transform not valid')
def init_cb(self, b):
if b.data:
self.init_state = True
else:
self.init_state = False
# DATA TRANSFORMATION
def convert_to_baselink(self, imu_msg, imu_model, transform):
# convert imu_msg from its frame to baselink frame
# Assumption! TODO: I'm going to assume that the axis are aligned between frames to start
# come back to this later, and rotate to align axis first
# proposed path:
# -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link
# -> run the same code below
'''
[sensor_msgs/Imu]:
std_msgs/Header header - DONE
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration - DONE
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance - DONE
'''
new_msg = Imu()
# Header
new_msg.header = imu_msg.header
new_msg.header.frame_id = '/base_link'
# Orientation (same based on Assumption! above)
new_msg.orientation = imu_msg.orientation
# including covariance, because same. will likely drop for rotation
new_msg.orientation_covariance = imu_msg.orientation_covariance
# Angular Velocity (same based on Assumption! above)
new_msg.angular_velocity = imu_msg.angular_velocity
# including covariance, because same. will likely drop for rotation
new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance
# Linear Acceleration (not the same, even with assumption)
new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform)
return new_msg
def solid_body_translate_lin_acc(self, lin_acc, state, transform):
# TODO(buckbaskin): check dynamics notes
# Free Moving Rigid Body Kinematics
# A-p = A-o + w-dot x R_prime-p + w x (w x R_prime-p)
# We have A-p (the linear acceleration from the Imu)
# We are solving for A-o (the acceleration for the baselink frame)
# angular velocity is consistent across the rigid body (measured at Imu)
# angular acceleration (w-dot) is interpolated from velocity data
# R-p is the vector between the two (transform)
# known: A-p, w-dot, w, R-p
# solving for: A-o
# A-o = A-p - w-dot x R-p - w x ( w x R-p )
Ap = Vector.from_Vector3(lin_acc) # v is a state_model.Vector
position = transform[0]
r = Vector.from_tf_position(position)
Ap = lin_acc
w_dot = state.alpha()
w = state.omega()
Ao = (Ap
.minus(w_dot.cross(r))
.minus(w.cross(w.cross(r))))
return Ao.to_Vector3()
# HELPER FUNCTIONS
def theta_avg(self, twerk, theta0, w0, a0, dt):
# twerk aka twisting jerk
return twerk*pow(dt,3)/24 + a0*pow(dt,2)/6 + w0*dt/2 + theta0
def twerk(self, theta0, theta1, w0, a0, dt):
twerk = ((((((theta1-theta0) / dt) - w0) / (dt / 2)) - a0) / (dt / 6))
return twerk
def ang_acc(self, twerk, a0, dt):
a1 = a0 + twerk*dt
return Vector(0,0,a1)
def lin_vel(self, a_measured, v0, dt):
v1 = v0 + a_measured*dt
return Vector(v1, 0, 0)
def linear_shift(self, x0, y0, theta, v0, a, dt):
v_avg = v0 + (v0 + a*dt)
dx = v_avg*math.cos(theta)
dy = v_avg*math.sin(theta)
return Vector(x0+dx, y0+dy, 0)
def quat_to_euler(self, quaternion):
# returns the heading from a given quaternion
q = quaternion
q_array = [q.x, q.y, q.z, q.w]
return euler_from_quaternion(q_array)[2]
def euler_to_quat(self, euler_heading):
# returns Quaternion ros message
q = quaternion_from_euler(0,0,euler_heading)
return Quaternion(w=q[3], x=q[0], y=q[1], z=q[2])
class Controller:
Idle = 0
Automatic = 1
TakingOff = 2
Landing = 3
def __init__(self, frame):
self.frame = frame
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
self.pidX = PID(35, 10, 0.0, -20, 20, "x")
self.pidY = PID(-35, -10, -0.0, -20, 20, "y")
self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Idle
self.goal = Pose()
rospy.Subscriber("goal", Pose, self._poseChanged)
rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff)
rospy.Service("land", std_srvs.srv.Empty, self._land)
def getTransform(self, source_frame, target_frame):
now = rospy.Time.now()
success = False
if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)):
t = self.listener.getLatestCommonTime(source_frame, target_frame)
if self.listener.canTransform(source_frame, target_frame, t):
position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t)
success = True
delta = (now - t).to_sec() * 1000 #ms
if delta > 25:
rospy.logwarn("Latency: %f ms.", delta)
# self.listener.clear()
# rospy.sleep(0.02)
if success:
return position, quaternion, t
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _poseChanged(self, data):
self.goal = data
def _takeoff(self, req):
rospy.loginfo("Takeoff requested!")
self.state = Controller.TakingOff
return std_srvs.srv.EmptyResponse()
def _land(self, req):
rospy.loginfo("Landing requested!")
self.state = Controller.Landing
return std_srvs.srv.EmptyResponse()
def run(self):
thrust = 0
while not rospy.is_shutdown():
now = rospy.Time.now()
if self.state == Controller.TakingOff:
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] > 0.05 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 = Twist()
msg.linear.z = thrust
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Landing:
self.goal.position.z = 0.05
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] <= 0.1:
self.state = Controller.Idle
msg = Twist()
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Automatic or self.state == Controller.Landing:
# transform target world coordinates into local coordinates
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
targetWorld = PoseStamped()
targetWorld.header.stamp = t
targetWorld.header.frame_id = "world"
targetWorld.pose = self.goal
targetDrone = self.listener.transformPose(self.frame, 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 = Twist()
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)
msg.angular.z = self.pidYaw.update(0.0, euler[2])
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Idle:
msg = Twist()
self.pubNav.publish(msg)
rospy.sleep(0.01)
class GripperMarkers:
_offset = 0.09
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
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']
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
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()
else:
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.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def get_ee_pose(self):
from_frame = 'base_link'
to_frame = self.side_prefix + '_wrist_roll_link'
try:
if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame):
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
# t = rospy.Time.now()
(pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :(
else:
rospy.logerr('TF frames do not exist; could not get end effector pose.')
except Exception as err:
rospy.logerr('Could not get end effector pose through TF.')
rospy.logerr(err)
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, feedback):
rospy.loginfo('You pressed the `Move arm here` button from the menu.')
'''Moves the arm to the desired joints'''
print feedback
time_to_joint = 2.0
positions = self.ik.get_ik_for_ee(feedback.pose)
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (self.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)
pass
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_' + self.side_prefix
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]))
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
print self
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution is 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
class SimAdapter(object):
def __init__(self):
pass
def start(self):
rospy.init_node('sim_adapter')
self.init_params()
self.init_state()
self.init_vars()
self.init_publishers()
self.init_subscribers()
self.init_timers()
rospy.spin()
def init_params(self):
self.param_model = rospy.get_param("~model", "simple_nonlinear")
self.rate = rospy.get_param("~rate", 40)
self.publish_state_estimate = rospy.get_param("~publish_state_estimate", True)
self.publish_odometry_messages = rospy.get_param("~publish_odometry", True)
self.sim_odom_topic = rospy.get_param("~sim_odom_topic", "odom")
def init_state(self):
if self.param_model == "simple_nonlinear":
self.model = SimpleNonlinearModel()
elif self.param_model == "linear":
self.model = LinearModel()
else:
rospy.logerror("Model type '%s' unknown" % self.model)
raise Exception("Model type '%s' unknown" % self.model)
def init_vars(self):
self.latest_cmd_msg = control_mode_output()
self.motor_enable = False
self.thrust_cmd = 0.0
self.tfl = TransformListener()
self.T_vicon_imu = None
self.T_imu_vicon = None
self.T_enu_ned = None
def init_publishers(self):
# Publishers
if self.publish_odometry_messages:
self.pub_odom = rospy.Publisher(self.sim_odom_topic, Odometry)
if self.publish_state_estimate:
self.pub_state = rospy.Publisher('state', TransformStamped)
def init_subscribers(self):
# Subscribers
self.control_input_sub = rospy.Subscriber('controller_mux/output', control_mode_output, self.control_input_callback)
self.motor_enable_sub = rospy.Subscriber('teleop_flyer/motor_enable', Bool, self.motor_enable_callback)
def init_timers(self):
self.simulation_timer = rospy.Timer(rospy.Duration(1.0/self.rate), self.simulation_timer_callback)
# Subscriber callbacks:
def control_input_callback(self, msg):
rospy.logdebug('Current command is: ' + str(msg))
self.latest_cmd_msg = msg
def motor_enable_callback(self, msg):
if msg.data != self.motor_enable:
#rospy.loginfo('Motor enable: ' + str(msg.data))
self.motor_enable = msg.data
# Timer callbacks:
def simulation_timer_callback(self, event):
if False:
print event.__dict__
# print 'last_expected: ', event.last_expected
# print 'last_real: ', event.last_real
# print 'current_expected: ', event.current_expected
# print 'current_real: ', event.current_real
# print 'current_error: ', (event.current_real - event.current_expected).to_sec()
# print 'profile.last_duration:', event.last_duration.to_sec()
# if event.last_real:
# print 'last_error: ', (event.last_real - event.last_expected).to_sec(), 'secs'
# print
if event.last_real is None:
dt = 0.0
else:
dt = (event.current_real - event.last_real).to_sec()
self.update_controller(dt)
self.update_state(dt)
#rospy.loginfo("position: " + str(self.position) + " velocity: " + str(self.velocity) + " thrust_cmd: " + str(self.thrust_cmd))
if self.publish_odometry_messages:
self.publish_odometry()
if self.publish_state_estimate:
self.publish_state(event.current_real)
def update_state(self, dt):
# The following model is completely arbitrary and should not be taken to be representative of
# real vehicle performance!
# But, it should be good enough to test out control modes etc.
self.model.update(dt)
def update_controller(self, dt):
lcm = self.latest_cmd_msg
self.model.set_input(lcm.motors_on, lcm.roll_cmd, lcm.pitch_cmd, lcm.direct_yaw_rate_commands,
lcm.yaw_cmd, lcm.yaw_rate_cmd, lcm.direct_thrust_commands, lcm.alt_cmd, lcm.thrust_cmd)
#rospy.loginfo("thrust_cmd = %f, dt = %f" % (self.thrust_cmd, dt))
def publish_odometry(self):
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "/ned"
odom_msg.child_frame_id = "/simflyer1/flyer_imu"
oppp = odom_msg.pose.pose.position
oppp.x, oppp.y, oppp.z = self.model.get_position()
ottl = odom_msg.twist.twist.linear
ottl.x, ottl.y, ottl.z = self.model.get_velocity()
oppo = odom_msg.pose.pose.orientation
oppo.x, oppo.y, oppo.z, oppo.w = self.model.get_orientation()
otta = odom_msg.twist.twist.angular
otta.x, otta.y, otta.z = self.model.get_angular_velocity()
self.pub_odom.publish(odom_msg)
def publish_state(self, t):
state_msg = TransformStamped()
t_ned_imu = tft.translation_matrix(self.model.get_position())
R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
#rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
if self.T_imu_vicon is None:
# grab the static transform from imu to vicon frame from param server:
frames = rospy.get_param("frames", None)
ypr = frames['vicon_to_imu']['rotation']
q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
# rospy.loginfo(str(R_vicon_imu))
# rospy.loginfo(str(t_vicon_imu))
self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
rospy.loginfo(str(self.T_enu_ned))
rospy.loginfo(str(self.T_imu_vicon))
#rospy.loginfo(str(T_vicon_imu))
# we have /ned -> /imu, need to output /enu -> /vicon:
T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
state_msg.header.stamp = t
state_msg.header.frame_id = "/enu"
state_msg.child_frame_id = "/simflyer1/flyer_vicon"
stt = state_msg.transform.translation
stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
stro = state_msg.transform.rotation
stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
self.pub_state.publish(state_msg)
def get_transform(self, tgt, src):
t = self.tfl.getLatestCommonTime(tgt, src)
t_src_tgt, q_src_tgt = self.tfl.lookupTransform(tgt, src, t)
T_src_tgt =self.tfl.fromTranslationRotation(t_src_tgt, q_src_tgt)
return T_src_tgt
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()
# Code for moving the robots joints
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']
# 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()
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.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]
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.')
joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
print 'joint: ' + str(joint_positions)
if(joint_positions != None):
self.toggle_arm(self.side_prefix, 'Freeze', False)
#joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
print 'returned from IK: ' + str(joint_positions)
#print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
#joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
print 'done'
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
print 'toggle'
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 move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(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)
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]))
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
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_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)
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 SinglePersonTracker:
MAX_LOCK_ON_DIST = 2.0
EXPECTED_STARTING_POSITION_X = 2.0
EXPECTED_STARTING_POSITION_Y = 0.0
def __init__(self):
self.tf = TransformListener()
self.person_position = Point()
self.person_position.x = self.EXPECTED_STARTING_POSITION_X
self.person_position.y = self.EXPECTED_STARTING_POSITION_Y
self.locked_on = False
self.person_id = -1
self.single_person_tracked_pub = rospy.Publisher(
'/frg_point_follower/point2', Point)
self.people_tracked_sub = rospy.Subscriber(
'people_tracked', PoseArray, self.tracked_people_callback)
self.FIXED_FRAME = None
if len(sys.argv) == 4:
self.FIXED_FRAME = sys.argv[1]
else:
print "ERRORS: incorrect number of arguments passed to kalman_confidence_person_tracker"
def tracked_people_callback(self, msg):
rospy.loginfo(self.person_position.x)
if self.locked_on:
# If we're locked on to one person, just use their ID to find their position
person_found = False
for pose in msg.poses:
if pose.position.z == self.person_id:
person_found = True
new_pose = PoseStamped()
new_pose.pose.position.x = pose.position.x
new_pose.pose.position.y = pose.position.y
new_pose.header.frame_id = msg.header.frame_id
new_pose.header.stamp = self.tf.getLatestCommonTime(
"base_footprint", "odom")
pose_tf = self.tf.transformPose("base_footprint", new_pose)
self.person_position.x = pose_tf.pose.position.x
self.person_position.y = pose_tf.pose.position.y
break
if not person_found:
self.locked_on = False
rospy.loginfo("Lost our person")
rospy.loginfo(self.locked_on)
else:
# If we haven't locked onto to someone, find the closest person to last locked on position
closest_dist = self.MAX_LOCK_ON_DIST
closest_x = None
closest_y = None
closest_id = None
for pose in msg.poses:
new_pose = PoseStamped()
new_pose.pose.position.x = pose.position.x
new_pose.pose.position.y = pose.position.y
new_pose.header.frame_id = msg.header.frame_id
new_pose.header.stamp = self.tf.getLatestCommonTime(
"base_footprint", "odom")
pose_tf = self.tf.transformPose("base_footprint", new_pose)
rel_x = pose_tf.pose.position.x - self.person_position.x
rel_y = pose_tf.pose.position.y - self.person_position.y
dist = (rel_x**2 + rel_y**2)**(1. / 2.)
if dist < closest_dist:
closest_dist = dist
closest_x = pose_tf.pose.position.x
closest_y = pose_tf.pose.position.y
closest_id = pose.position.z # Note the z position is actually the ID num
self.locked_on = True
if self.locked_on:
self.person_position.x = closest_x
self.person_position.y = closest_y
self.person_id = closest_id
rospy.loginfo("Locked on to a person")
def publish_person(self):
if self.locked_on:
#rospy.logwarn('hello2')
person_position_offset = self.person_position
person_position_offset.y -= 0.15
self.single_person_tracked_pub.publish(person_position_offset)
# else:
# # Publish a point at (0,0,0) if we're not locked onto a person
# dummy_point = Point()
# dummy_point.x = 0.0
# dummy_point.y = 0.0
# dummy_point.z = 0.0
# self.single_person_tracked_pub.publish(dummy_point)
def spin(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.publish_person()
rate.sleep()
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 SocialGaze:
gaze_goal_strs = {
GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD',
GazeGoal.FOLLOW_RIGHT_EE: 'FOLLOW_RIGHT_EE',
GazeGoal.FOLLOW_LEFT_EE: 'FOLLOW_LEFT_EE',
GazeGoal.GLANCE_RIGHT_EE: 'GLANCE_RIGHT_EE',
GazeGoal.GLANCE_LEFT_EE: 'GLANCE_LEFT_EE',
GazeGoal.NOD: 'NOD',
GazeGoal.SHAKE: 'SHAKE',
GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE',
GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT',
GazeGoal.LOOK_DOWN: 'LOOK_DOWN',
GazeGoal.NOD_ONCE: 'NOD_ONCE ',
GazeGoal.SHAKE_ONCE: 'SHAKE_ONCE',
GazeGoal.FREEZE: 'FREEZE',
GazeGoal.RELAX: 'RELAX',
}
'''This maps gaze goal constants to human-readable forms.'''
def __init__(self):
self.defaultLookatPoint = Point(1, 0, 1.3)
self.downLookatPoint = Point(0.5, 0, 0.5)
self.targetLookatPoint = Point(1, 0, 1.3)
self.currentLookatPoint = Point(1, 0, 1.3)
self.currentGazeAction = GazeGoal.LOOK_FORWARD
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint)
self.isFrozen = False
# 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'
toFrame = '/r_gripper_l_finger_tip_link'
else:
#toFrame = '/l_wrist_roll_link'
toFrame = '/l_gripper_l_finger_tip_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.isFrozen and command != GazeGoal.RELAX:
self.gazeActionServer.set_aborted()
return
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
elif (command == GazeGoal.FREEZE):
self.isFrozen = True
self.isActionComplete = True
elif (command == GazeGoal.RELAX):
self.isFrozen = False
self.isActionComplete = True
rospy.loginfo('\tSetting gaze action to: ' +
SocialGaze.gaze_goal_strs[command])
self.currentGazeAction = command
while (not self.isActionComplete):
time.sleep(0.1)
#self.currentGazeAction = None
# Perturb the head goal so it gets updated in the update loop.
self.headGoal.target.point.x += 0.01
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):
if not self.isFrozen:
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.currentGazeAction is None:
pass
elif (self.isTheSame(self.point2array(self.headGoal.target.point),
self.point2array(self.targetLookatPoint))):
if (isActionPossiblyComplete):
if (self.headActionClient.get_state() == GoalStatus.SUCCEEDED):
self.isActionComplete = True
else:
self.headGoal.target.point.x = self.currentLookatPoint.x
self.headGoal.target.point.y = self.currentLookatPoint.y
self.headGoal.target.point.z = self.currentLookatPoint.z
self.headActionClient.send_goal(self.headGoal)
time.sleep(0.02)
class yoboticsOdometry:
def __init__(self):
rospy.Subscriber("/yobotics/gazebo/contacts", Contacts,
self.contacts_callback)
self.odom_publisher = rospy.Publisher("odom/raw",
Odometry,
queue_size=50)
self.odom_broadcaster = tf.TransformBroadcaster()
self.tf = TransformListener()
self.foot_links = [
"lf_foot_link", "rf_foot_link", "lh_foot_link", "rh_foot_link"
]
self.nominal_foot_positions = [[0, 0], [0, 0], [0, 0], [0, 0]]
self.prev_foot_positions = [[0, 0], [0, 0], [0, 0], [0, 0]]
self.prev_theta = [0, 0, 0, 0]
self.prev_stance_angles = [0, 0, 0, 0]
self.prev_time = 0
self.pos_x = 0
self.pos_y = 0
self.theta = 0
self.publish_odom_tf(0, 0, 0, 0)
rospy.sleep(1)
for i in range(4):
self.nominal_foot_positions[i] = self.get_foot_position(i)
self.prev_foot_positions[i] = self.nominal_foot_positions[i]
self.prev_theta[i] = math.atan2(self.prev_foot_positions[i][1],
self.prev_foot_positions[i][0])
def publish_odom(self, x, y, z, theta, vx, vy, vth):
current_time = rospy.Time.now()
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom_quat = tf.transformations.quaternion_from_euler(0, 0, theta)
odom.pose.pose = Pose(Point(x, y, z), Quaternion(*odom_quat))
odom.child_frame_id = "base_footprint"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
# publish the message
self.odom_publisher.publish(odom)
def publish_odom_tf(self, x, y, z, theta):
current_time = rospy.Time.now()
odom_quat = quaternion_from_euler(0, 0, theta)
self.odom_broadcaster.sendTransform((x, y, z), odom_quat, current_time,
"base_footprint", "odom")
def get_foot_position(self, leg_id):
if self.tf.frameExists("base_link" and self.foot_links[leg_id]):
t = self.tf.getLatestCommonTime("base_link",
self.foot_links[leg_id])
position, quaternion = self.tf.lookupTransform(
"base_link", self.foot_links[leg_id], t)
return position
else:
return 0, 0, 0
def contacts_callback(self, data):
self.leg_contact_states = data.contacts
def is_almost_equal(self, a, b, max_rel_diff):
diff = abs(a - b)
a = abs(a)
b = abs(b)
largest = 0
if b > a:
largest = b
else:
larget = a
if diff <= (largest * max_rel_diff):
return True
return False
def run(self):
while not rospy.is_shutdown():
leg_contact_states = self.leg_contact_states
current_foot_position = [[0, 0], [0, 0], [0, 0], [0, 0]]
stance_angles = [0, 0, 0, 0]
current_theta = [0, 0, 0, 0]
delta_theta = 0
in_xy = False
total_contact = sum(leg_contact_states)
x_sum = 0
y_sum = 0
theta_sum = 0
for i in range(4):
current_foot_position[i] = self.get_foot_position(i)
for i in range(4):
current_theta[i] = math.atan2(current_foot_position[i][1],
current_foot_position[i][0])
from_nominal_x = self.nominal_foot_positions[i][
0] - current_foot_position[i][0]
from_nominal_y = self.nominal_foot_positions[i][
1] - current_foot_position[i][1]
stance_angles[i] = math.atan2(from_nominal_y, from_nominal_x)
# print stance_angles
#check if it's moving in the x or y axis
if self.is_almost_equal(stance_angles[i], abs(1.5708),
0.001) or self.is_almost_equal(
stance_angles[i], abs(3.1416),
0.001):
in_xy = True
if current_foot_position[i] != None and leg_contact_states[
i] == True and total_contact == 2:
delta_x = (self.prev_foot_positions[i][0] -
current_foot_position[i][0]) / 2
delta_y = (self.prev_foot_positions[i][1] -
current_foot_position[i][1]) / 2
x = delta_x * math.cos(self.theta) - delta_y * math.sin(
self.theta)
y = delta_x * math.sin(self.theta) + delta_y * math.cos(
self.theta)
x_sum += delta_x
y_sum += delta_y
self.pos_x += x
self.pos_y += y
if not in_xy:
delta_theta = self.prev_theta[i] - current_theta[i]
theta_sum += delta_theta
self.theta += delta_theta / 2
now = rospy.Time.now().to_sec()
dt = now - self.prev_time
vx = x_sum / dt
vy = y_sum / dt
vth = theta_sum / dt
self.publish_odom(self.pos_x, self.pos_y, 0, self.theta, vx, vy,
vth)
# self.publish_odom_tf(self.pos_x, self.pos_y, 0, self.theta)
self.prev_foot_positions = current_foot_position
self.prev_theta = current_theta
self.prev_stance_angles = stance_angles
self.prev_time = now
rospy.sleep(0.01)
class ArucoFilter(object):
def __init__(self):
rospy.loginfo("Initializing...")
# Node Hz rate
self.rate = 10
self.rate_changed = False
self.global_frame_id = '/world'
self.topic_name = '/aruco_filtered'
self.initial_pose_head_x = 1.0
self.initial_pose_head_y = 0.0
self.initial_pose_head_z = 0.6
self.initial_pose = Pose()
self.tf_l = TransformListener()
rospy.sleep(3.0) # Let the subscriber to its job
self.dyn_rec_srv = Server(ArucoFilterConfig, self.dyn_rec_callback)
self.last_pose = None
self.new_pose = False
self.pose_sub = rospy.Subscriber('/aruco_single/pose',
PoseStamped,
self.pose_cb,
queue_size=1)
rospy.loginfo("Subscribed to: " + str(self.pose_sub.resolved_name))
self.last_goal_timestamp = rospy.Time.now()
self.pose_pub = rospy.Publisher(self.topic_name,
PoseStamped,
queue_size=1)
self.head_pub = rospy.Publisher('/whole_body_kinematic_controler/gaze_objective_stereo_optical_frame_goal',
PoseStamped,
queue_size=1)
self.point_head_pub = rospy.Publisher('/head_controller/point_head_action/goal',
PointHeadActionGoal,
queue_size=1)
self.wrist_pub = rospy.Publisher('/whole_body_kinematic_controler/wrist_right_ft_link_goal',
PoseStamped,
queue_size=1)
self.hand_pub = rospy.Publisher(
'/right_hand_controller/command', JointTrajectory, queue_size=1)
rospy.sleep(1.0)
self.run()
def pose_cb(self, data):
self.last_pose = data
self.new_pose = True
def run(self):
r = rospy.Rate(self.rate)
rospy.loginfo("Node running!")
while not rospy.is_shutdown():
# To change the node rate
if self.rate_changed:
r = rospy.Rate(self.rate)
self.rate_changed = False
self.publish_goal()
r.sleep()
def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"):
ps = PoseStamped()
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
if from_frame[0] == '/':
ps.header.frame_id = from_frame[1:]
else:
ps.header.frame_id = from_frame
# TODO: check pose being PoseStamped or Pose
ps.pose = pose
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_ps = self.tf_l.transformPose(self.global_frame_id, ps)
transform_ok = True
return world_ps
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
ps.header.stamp = self.tf_l.getLatestCommonTime(
self.global_frame_id, from_frame)
def send_head_point_goal(self, pose):
g = PointHeadActionGoal()
t = g.goal.target
t.header.frame_id = pose.header.frame_id
t.point = pose.pose.position
g.goal.pointing_axis.z = 1.0
g.goal.pointing_frame = 'stereo_optical_frame'
g.goal.min_duration = rospy.Duration(0.5)
g.goal.max_velocity = 1.0
self.point_head_pub.publish(g)
def publish_goal(self):
if not self.new_pose and rospy.Time.now() - self.last_goal_timestamp > rospy.Duration(self.goal_timeout):
# Send home goal
ps = PoseStamped()
ps.header.frame_id = self.global_frame_id
ps.pose = deepcopy(self.initial_pose)
self.pose_pub.publish(ps)
if self.send_wrist_goals:
self.wrist_pub.publish(ps)
if self.send_head_goals:
ps_head = PoseStamped()
ps_head.header.frame_id = self.global_frame_id
ps_head.pose.position.x = self.initial_pose_head_x
ps_head.pose.position.y = self.initial_pose_head_y
ps_head.pose.position.z = self.initial_pose_head_z
ps_head.pose.orientation.w = 1.0
self.head_pub.publish(ps_head)
self.send_head_point_goal(ps_head)
self.thumbs_up_hand()
else:
if self.new_pose:
self.new_pose = False
if self.last_pose.header.frame_id != self.global_frame_id:
rospy.loginfo("Goal in different frame, (" +
self.last_pose.header.frame_id +
") transforming to " + self.global_frame_id)
goal_pose = self.transform_pose_to_world(
self.last_pose.pose, self.last_pose.header.frame_id)
else:
goal_pose = deepcopy(self.last_pose)
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position.x = self.sanitize(goal_pose.pose.position.x,
self.min_x,
self.max_x)
goal_pose.pose.position.y = self.sanitize(goal_pose.pose.position.y,
self.min_y,
self.max_y)
goal_pose.pose.position.z = self.sanitize(goal_pose.pose.position.z,
self.min_z,
self.max_z)
# TODO: deal with orientation
goal_pose.pose.orientation = deepcopy(self.initial_pose.orientation)
if self.use_marker_orientation:
# Add roll to the hand, which is yaw in the marker, which will
# be in the wrist frame Z, so yaw too.
o1 = self.initial_pose.orientation
o2 = self.last_pose.pose.orientation
r1, p1, y1 = euler_from_quaternion(
[o1.x, o1.y, o1.z, o1.w])
r2, p2, y2 = euler_from_quaternion(
[o2.x, o2.y, o2.z, o2.w])
# rospy.loginfo("Marker rpy: " + str( (r2, p2, y2) ))
# rospy.loginfo("initialrpy: " + str( (r1, p1, y1) ))
q = quaternion_from_euler(r1 + (r2 + 1.57), p1 + (-y2), y1 + p2)# y1 + y2)
goal_pose.pose.orientation = Quaternion(*q)
self.pose_pub.publish(goal_pose)
if self.send_head_goals:
self.head_pub.publish(goal_pose)
self.send_head_point_goal(goal_pose)
if self.send_wrist_goals:
# Set offset for not covering the marker with the hand
# And not push the marker itself
goal_pose.pose.position.x -= 0.15
goal_pose.pose.position.z -= 0.1
self.wrist_pub.publish(goal_pose)
# Put hand in pointing pose
self.pointing_hand()
self.last_goal_timestamp = rospy.Time.now()
def sanitize(self, data, min_v, max_v):
if data > max_v:
return max_v
if data < min_v:
return min_v
return data
def dyn_rec_callback(self, config, level):
"""
:param config:
:param level:
:return:
"""
rospy.loginfo("Received reconf call: " + str(config))
# Node Hz rate
if self.rate != config['rate']:
self.rate_changed = True
self.rate = config['rate']
self.send_head_goals = config['send_head_goals']
self.send_wrist_goals = config['send_wrist_goals']
self.use_marker_orientation = config['use_marker_orientation']
# Limits
self.min_x = config['min_x']
self.max_x = config['max_x']
self.min_y = config['min_y']
self.max_y = config['max_y']
self.min_z = config['min_z']
self.max_z = config['max_z']
self.initial_pose_x = config['initial_pose_x']
self.initial_pose_y = config['initial_pose_y']
self.initial_pose_z = config['initial_pose_z']
self.initial_pose.position.x = self.initial_pose_x
self.initial_pose.position.y = self.initial_pose_y
self.initial_pose.position.z = self.initial_pose_z
self.initial_pose_roll_degrees = config['initial_pose_roll_degrees']
self.initial_pose_pitch_degrees = config['initial_pose_pitch_degrees']
self.initial_pose_yaw_degrees = config['initial_pose_yaw_degrees']
q = quaternion_from_euler(radians(self.initial_pose_roll_degrees),
radians(self.initial_pose_pitch_degrees),
radians(self.initial_pose_yaw_degrees))
self.initial_pose.orientation = Quaternion(*q)
self.initial_pose_head_x = config['initial_pose_head_x']
self.initial_pose_head_y = config['initial_pose_head_y']
self.initial_pose_head_z = config['initial_pose_head_z']
if config['global_frame_id'][0] != '/':
config['global_frame_id'] = '/' + config['global_frame_id']
if config['global_frame_id'] != self.global_frame_id:
self.global_frame_id = config['global_frame_id']
if self.topic_name != config["topic_name"]:
self.topic_name = config["topic_name"]
self.follow_sub = rospy.Subscriber(self.topic_name,
PoseStamped,
self.follow_pose_cb,
queue_size=1)
self.goal_timeout = config['goal_timeout']
return config
def pointing_hand(self):
jt = JointTrajectory()
jt.joint_names = [
'hand_right_thumb_joint', 'hand_right_index_joint', 'hand_right_mrl_joint']
jtp = JointTrajectoryPoint()
# Hardcoded joint limits
jtp.positions = [5.5, 0.0, 6.0]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
# Send goal
self.hand_pub.publish(jt)
def open_hand(self):
jt = JointTrajectory()
jt.joint_names = [
'hand_right_thumb_joint', 'hand_right_index_joint', 'hand_right_mrl_joint']
jtp = JointTrajectoryPoint()
# Hardcoded joint limits
jtp.positions = [0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
# Send goal
self.hand_pub.publish(jt)
def thumbs_up_hand(self):
jt = JointTrajectory()
jt.joint_names = [
'hand_right_thumb_joint', 'hand_right_index_joint', 'hand_right_mrl_joint']
jtp = JointTrajectoryPoint()
# Hardcoded joint limits
jtp.positions = [0.0, 5.5, 6.0]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
# Send goal
self.hand_pub.publish(jt)
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.Imu = []
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.subImu = rospy.Subscriber(sub_imu_data, Imu, self.ImuCallback)
self.subState = rospy.Subscriber(sub_name, String, self.CfsCallback)
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]])
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 ScenarioServer(object):
_id = 0
_loaded = False
_robot_poses = []
_distances = [0,0]
def __init__(self, name):
rospy.loginfo("Starting scenario server")
self.robot = rospy.get_param("~robot", False)
conf_file = find_resource("topological_navigation", "scenario_server.yaml")[0]
rospy.loginfo("Reading config file: %s ..." % conf_file)
with open(conf_file,'r') as f:
conf = yaml.load(f)
self._robot_start_node = conf["robot_start_node"]
self._robot_goal_node = conf["robot_goal_node"]
self._obstacle_node_prefix = conf["obstacle_node_prefix"]
self._obstacle_types = conf["obstacle_types"]
self._timeout = conf["success_metrics"]["nav_timeout"]
rospy.loginfo(" ... done")
self._insert_maps()
self.tf = TransformListener()
rospy.Service("~load", LoadTopoNavTestScenario, self.load_scenario)
self.reset_pose = self.reset_pose_robot if self.robot else self.reset_pose_sim
rospy.Service("~reset", Empty, self.reset)
rospy.Service("~start", RunTopoNavTestScenario, self.start)
rospy.loginfo("All done")
def _insert_maps(self):
map_dir = rospy.get_param("~map_dir", "")
rospy.loginfo("Inserting maps into datacentre...")
if map_dir == "":
rospy.logwarn("No 'map_dir' given. Will not insert maps into datacentre and assume they are present already.")
return
y = YamlMapLoader()
y.insert_maps(y.read_maps(map_dir))
rospy.loginfo("... done")
def robot_callback(self, msg):
self._robot_poses.append(msg)
def _connect_port(self, port):
""" Establish the connection with the given MORSE port"""
sock = None
for res in socket.getaddrinfo(HOST, port, socket.AF_UNSPEC, socket.SOCK_STREAM):
af, socktype, proto, canonname, sa = res
try:
sock = socket.socket(af, socktype, proto)
except socket.error:
sock = None
continue
try:
sock.connect(sa)
except socket.error:
sock.close()
sock = None
continue
break
return sock
def _translate(self, msg, target_tf):
t = self.tf.getLatestCommonTime(target_tf, msg.header.frame_id)
msg.header.stamp=t
new_pose=self.tf.transformPose(target_tf,msg)
return new_pose
def _clear_costmaps(self):
try:
s = rospy.ServiceProxy("/move_base/clear_costmaps", Empty)
s.wait_for_service()
s()
except rospy.ServiceException as e:
rospy.logerr(e)
def _init_nav(self, pose):
pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
rospy.sleep(1.0)
initialPose = PoseWithCovarianceStamped()
initialPose.header = pose.header
initialPose.pose.pose = pose.pose
p_cov = np.array([0.0]*36)
initialPose.pose.covariance = tuple(p_cov.ravel().tolist())
pub.publish(initialPose)
def _get_set_object_pose_command(self, agent, id, pose):
new_pose = self._translate(
msg=pose,
target_tf="/world"
)
return 'id%d simulation set_object_pose ["%s", "[%f, %f, 0.1]", "[%f, %f, %f, %f]"]\n' \
% (id, agent, new_pose.pose.position.x, new_pose.pose.position.y, \
new_pose.pose.orientation.w, new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z)
def robot_start_dist(self, msg):
self._distances = self._get_pose_distance(msg, self._robot_start_pose.pose)
def reset_pose_robot(self):
rospy.loginfo("Enabling freerun ...")
try:
s = rospy.ServiceProxy("/enable_motors", EnableMotors)
s.wait_for_service()
s(False)
except (rospy.ROSInterruptException, rospy.ServiceException) as e:
rospy.logwarn(e)
rospy.loginfo("... enabled")
while self._robot_start_node != rospy.wait_for_message("/current_node", String).data and not rospy.is_shutdown():
rospy.loginfo("+++ Please push the robot to '%s' +++" % self._robot_start_node)
rospy.sleep(1)
rospy.loginfo("+++ Robot at '%s' +++" % self._robot_start_node)
while not rospy.is_shutdown():
sub = rospy.Subscriber("/robot_pose", Pose, self.robot_start_dist)
rospy.loginfo("+++ Please confirm correct positioning with A button on joypad: distance %.2fm %.2fdeg +++" %(self._distances[0], self._distances[1]))
if self._robot_start_node != rospy.wait_for_message("/current_node", String).data:
self.reset_pose()
return
try:
if rospy.wait_for_message("/teleop_joystick/action_buttons", action_buttons, timeout=1.).A:
break
except rospy.ROSException:
pass
sub.unregister()
sub = None
rospy.loginfo("+++ Position confirmed +++")
rospy.loginfo("Enabling motors, resetting motor stop and barrier stopped ...")
try:
s = rospy.ServiceProxy("/enable_motors", EnableMotors)
s.wait_for_service()
s(True)
s = rospy.ServiceProxy("/reset_motorstop", ResetMotorStop)
s.wait_for_service()
s()
s = rospy.ServiceProxy("/reset_barrier_stop", ResetBarrierStop)
s.wait_for_service()
s()
except (rospy.ROSInterruptException, rospy.ServiceException) as e:
rospy.logwarn(e)
rospy.loginfo("... enabled and reset")
def _send_socket(self, sock, agent, pose):
sock.send(
self._get_set_object_pose_command(
agent,
self._id,
pose
)
)
res = sock.recv(4096)
self._id += 1
if "FAILURE" in res:
raise Exception(res)
def _get_quaternion_distance(self, q1, q2):
q1 = (q1.x, q1.y, q1.z, q1.w)
q2 = (q2.x, q2.y, q2.z, q2.w)
return (trans.euler_from_quaternion(q1)[2] - trans.euler_from_quaternion(q2)[2]) * 180/np.pi
def _get_euclidean_distance(self, p1, p2):
return np.sqrt(np.power(p2.x - p1.x, 2) + np.power(p2.y - p1.y, 2))
def _get_pose_distance(self, p1, p2):
e = self._get_euclidean_distance(p1.position, p2.position)
q = self._get_quaternion_distance(p1.orientation, p2.orientation)
return e, q
def reset_pose_sim(self):
sock = self._connect_port(PORT)
if not sock:
raise Exception("Could not create socket connection to morse")
# Clean up whole scene
# Create pose outside of map
clear_pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id="/map"),
pose=Pose(position=Point(x=20, y=20, z=0.01))
)
# Moving obstacles to clear_pose
for obstacle in self._obstacle_types:
for idx in range(10):
self._send_socket(sock, obstacle+str(idx), clear_pose)
# Setting robot and obstacles to correct position
self._send_socket(sock, "robot", self._robot_start_pose)
if len(self._obstacle_poses) > 0:
for idx, d in enumerate(self._obstacle_poses):
try:
obstacle = max([x for x in self._obstacle_types if x in d["name"]], key=len)
except ValueError:
rospy.logwarn("No obstacle specified for obstacle node '%s', will use '%s'." % (d["name"], self._obstacle_types[0]))
obstacle = self._obstacle_types[0]
rospy.loginfo("Adding obstacle %s%i" % (obstacle,idx))
self._send_socket(sock, obstacle+str(idx), d["pose"])
else:
rospy.logwarn("No nodes starting with '%s' found in map. Assuming test without obstacles." % self._obstacle_node_prefix)
sock.close()
while not rospy.is_shutdown():
rpose = rospy.wait_for_message("/robot_pose", Pose)
rospy.loginfo("Setting initial amcl pose ...")
self._init_nav(self._robot_start_pose)
dist = self._get_pose_distance(rpose, self._robot_start_pose.pose)
if dist[0] < 0.1 and dist[1] < 10:
break
rospy.sleep(0.2)
# self._robot_start_pose.header.stamp = rospy.Time.now()
rospy.loginfo(" ... pose set.")
def reset(self, req):
if not self._loaded:
rospy.logfatal("No scenario loaded!")
return EmptyResponse()
self.client.cancel_all_goals()
rospy.loginfo("Resetting robot position...")
self.reset_pose()
self._clear_costmaps()
rospy.loginfo("... reset successful")
return EmptyResponse()
def graceful_fail(self):
res = False
closest_node = rospy.wait_for_message("/closest_node", String).data
rospy.loginfo("Closest node: %s" % closest_node)
if closest_node != self._robot_start_node:
rospy.loginfo("Using policy execution from %s to %s" % (closest_node, self._robot_start_node))
s = rospy.ServiceProxy("/get_simple_policy/get_route_to", GetRouteTo)
s.wait_for_service()
policy = s(self._robot_start_node)
self.client.send_goal(ExecutePolicyModeGoal(route=policy.route))
self.client.wait_for_result(timeout=rospy.Duration(self._timeout))
res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED
self.client.cancel_all_goals()
else:
rospy.loginfo("Using topo nav from %s to %s" % (closest_node, self._robot_start_node))
rospy.loginfo("Starting topo nav client...")
client = actionlib.SimpleActionClient("/topological_navigation", GotoNodeAction)
client.wait_for_server()
rospy.loginfo(" ... done")
client.send_goal(GotoNodeGoal(target=self._robot_start_node))
client.wait_for_result(timeout=rospy.Duration(self._timeout))
res = client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED
client.cancel_all_goals()
return res
def start(self, req):
rospy.loginfo("Starting test ...")
if not self._loaded:
rospy.logfatal("No scenario loaded!")
return RunTopoNavTestScenarioResponse(False, False)
self._robot_poses = []
grace_res = False
sub = rospy.Subscriber("/robot_pose", Pose, self.robot_callback)
rospy.loginfo("Sending goal to policy execution ...")
print self._policy.route
self.client.send_goal(ExecutePolicyModeGoal(route=self._policy.route))
t = time.time()
rospy.loginfo("... waiting for result ...")
print self._timeout
nav_timeout = not self.client.wait_for_result(timeout=rospy.Duration(self._timeout))
elapsed = time.time() - t
res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED
rospy.loginfo("... policy execution finished")
self.client.cancel_all_goals()
if not res:
rospy.loginfo("Attempting graceful death ...")
grace_res = self.graceful_fail()
rospy.loginfo("... dead")
sub.unregister()
sub = None
distance = self.get_distance_travelled(self._robot_poses)
rospy.loginfo("... test done")
return RunTopoNavTestScenarioResponse(
nav_success=res,
nav_timeout=nav_timeout,
graceful_fail=grace_res,
human_success=False,
min_distance_to_human=0,
distance_travelled=distance,
travel_time=elapsed,
mean_speed=distance/elapsed
)
def get_distance_travelled(self, poses):
distance = 0.0
for idx in range(len(poses))[1:]:
distance += self._get_euclidean_distance(poses[idx].position, poses[idx-1].position)
return distance
def load_scenario(self, req):
# No try except to have exception break the test
s = rospy.ServiceProxy("/topological_map_manager/switch_topological_map", GetTopologicalMap)
s.wait_for_service()
topo_map = s(GetTopologicalMapRequest(pointset=req.pointset)).map
self.pointset = req.pointset
self._robot_start_pose = None
self._obstacle_poses = []
found_end_node = False
for node in topo_map.nodes:
if node.name == self._robot_start_node:
self._robot_start_pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id="/map"),
pose=node.pose
)
elif node.name == self._robot_goal_node:
found_end_node = True
elif node.name.startswith(self._obstacle_node_prefix):
self._obstacle_poses.append({
"name": node.name.lower(),
"pose": PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id="/map"),
pose=node.pose
)
})
if self._robot_start_pose == None:
raise Exception("Topological map '%s' does not contain start node '%s'." % (req.pointset, self._robot_start_node))
if not found_end_node:
raise Exception("Topological map '%s' does not contain goal node '%s'." % (req.pointset, self._robot_goal_node))
rospy.loginfo("Starting policy execution client...")
# Has to be done here because the policy execution server waits for a topo map.
self.client = actionlib.SimpleActionClient("/topological_navigation/execute_policy_mode", ExecutePolicyModeAction)
self.client.wait_for_server()
rospy.loginfo(" ... started")
self._loaded = True
self.reset(None)
# No try except to have exception break the test
rospy.loginfo("Getting route ...")
s = rospy.ServiceProxy("/get_simple_policy/get_route_to", GetRouteTo)
s.wait_for_service()
self._policy = s(self._robot_goal_node)
rospy.loginfo(" ... done")
return LoadTopoNavTestScenarioResponse()
class GoToPose():
def __init__(self):
self.offset = .4
self.tf = TransformListener()
self.wander_msg = Twist()
self.wander=False
self.sect_1 = 0
self.sect_2 = 0
self.sect_3 = 0
self.ang = {0:0,001:-1.2,10:-1.2,11:-1.2,100:1.5,101:1.0,110:1.0,111:1.2}
self.fwd = {0:.25,1:0,10:0,11:0,100:0.1,101:0,110:0,111:0}
self.dbgmsg = {0:'Move forward',1:'Veer right',10:'Veer right',11:'Veer right',100:'Veer left',101:'Veer left',110:'Veer left',111:'Veer right'}
self.current_pos=None
self.current_quat=None
self.goal_sent = False
self.node_matrix=node
self.velocity_publisher = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)
self.sound_publisher = rospy.Publisher('/mobile_base/commands/sound', Sound, queue_size=10)
# What to do if shut down (e.g. Ctrl-C or failure)
rospy.on_shutdown(self.shutdown)
# Tell the action client that we want to spin a thread by default
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Wait for the action server to come up")
# Allow up to 5 seconds for the action server to come up
self.move_base.wait_for_server(rospy.Duration(5))
#Beep turtlebot
def makeNoise(self):
msg = Sound()
msg.value = Sound.ON
self.sound_publisher.publish(msg)
#All of the laser roaming stuff
def movement(self, sect1, sect2, sect3):
'''Uses the information known about the obstacles to move robot.
Parameters are class variables and are used to assign a value to
variable sect and then set the appropriate angular and linear
velocities, and log messages.
These are pblished and the sect variables are reset.'''
sect = int(str(self.sect_1) + str(self.sect_2) + str(self.sect_3))
rospy.loginfo("Sect = " + str(sect))
sign = 1
if sect ==111:
if random.uniform(-1, 1)> 0:
sign=1
else:
sign=-1
self.wander_msg.angular.z = sign*self.ang[sect]
self.wander_msg.linear.x = self.fwd[sect]
rospy.loginfo(self.dbgmsg[sect])
self.velocity_publisher.publish(self.wander_msg)
self.reset_sect()
def reset_sect(self):
'''Resets the below variables before each new scan message is read'''
self.sect_1 = 0
self.sect_2 = 0
self.sect_3 = 0
def laserCallback(self,scanmsg):
##Passes laser scan message to for_callback function of sub_obj.
if self.wander:
self.for_callback(scanmsg)
def for_callback(self,laserscan):
'''Passes laserscan onto function sort which gives the sect
variables the proper values. Then the movement function is run
with the class sect variables as parameters.
Parameter laserscan is received from callback function.'''
self.sort(laserscan)
self.movement(self.sect_1, self.sect_2, self.sect_3)
def sort(self, laserscan):
'''Goes through 'ranges' array in laserscan message and determines
where obstacles are located. The class variables sect_1, sect_2,
and sect_3 are updated as either '0' (no obstacles within 0.7 m)
or '1' (obstacles within 0.7 m)
Parameter laserscan is a laserscan message.'''
entries = len(laserscan.ranges)
for entry in range(0,entries):
if 0.4 < laserscan.ranges[entry] < 0.75:
self.sect_1 = 1 if (0 < entry < entries/3) else 0
self.sect_2 = 1 if (entries/3 < entry < entries/2) else 0
self.sect_3 = 1 if (entries/2 < entry < entries) else 0
rospy.loginfo("sort complete,sect_1: " + str(self.sect_1) + " sect_2: " + str(self.sect_2) + " sect_3: " + str(self.sect_3))
def reset_sect(self):
'''Resets the below variables before each new scan message is read'''
self.sect_1 = 0
self.sect_2 = 0
self.sect_3 = 0
#The bumper wandering stuff
def wander():
global bump
twist = Twist()
if bump==0:
str = "right bumper, turning left %s"%rospy.get_time()
rospy.loginfo(str)
self.turn(random_duration(), turn_speed)
elif bump==1:
str = "left bumper, turning right %s"%rospy.get_time()
rospy.loginfo(str)
self.turn(random_duration(), -turn_speed)
elif bump==2:
str = "both bumpers, turning left %s"%rospy.get_time()
rospy.loginfo(str)
self.turn(random_duration(), turn_speed)
else:
str = "moving straight ahead %s"%rospy.get_time()
#rospy.loginfo(str)
twist.linear.x = movement_speed; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
bump = -1
self.velocity_publisher.publish(twist)
rospy.sleep(action_duration)
def turn(duration, weight):
twist = Twist()
# First, back up slightly from the wall
twist.linear.x = -movement_speed; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
self.velocity_publisher.publish(twist)
rospy.sleep(action_duration)
# Now, keep turning until the end of the specified duration
currentTime = rospy.get_time();
stopTime = rospy.get_time() + duration;
while (rospy.get_time() < stopTime):
str = "turning %s"%rospy.get_time()
#rospy.loginfo(str)
twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = weight
self.velocity_publisher.publish(twist)
rospy.sleep(action_duration)
def twist(self,deg=360):
vel_msg = Twist()
PI = 3.1415926535897
# Receiveing the user's input
rospy.loginfo("Rotating In Place")
speed = 25
angle = deg
clockwise = True #True or ru
#Converting from angles to radians
angular_speed = speed*2*PI/360
relative_angle = angle*2*PI/360
#We wont use linear components
vel_msg.linear.x=0
vel_msg.linear.y=0
vel_msg.linear.z=0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
# Checking if our movement is CW or CCW
if clockwise:
vel_msg.angular.z = -abs(angular_speed)
else:
vel_msg.angular.z = abs(angular_speed)
# Setting the current time for distance calculus
t0 = rospy.Time.now().to_sec()
current_angle = 0
while(current_angle < 2.5*relative_angle):
self.velocity_publisher.publish(vel_msg)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed*(t1-t0)
#Forcing our robot to stop
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
rospy.loginfo("Done Rotating")
#Callback for tf subscriber -- updates tag information
def callback(self,data):
val = data.transforms
for i in val:
cf_id = i.child_frame_id
if 'tag_' in cf_id:
num = int(cf_id[4:])
#print "Found Tag # " + str(num)
if self.node_matrix[num]['navigatedTo'] == False:
self.node_matrix[num]['located'] = True
self.node_matrix[num]['pos'],self.node_matrix[num]['quat'] = self.tf.lookupTransform("/map", cf_id, self.tf.getLatestCommonTime("/map", cf_id))
rospy.loginfo("Found Tag" + str(num))
yaw = euler_from_quaternion(self.node_matrix[num]['quat'])[2]
self.node_matrix[num]['pos'][0] = self.node_matrix[num]['pos'][0] + sin(yaw)*self.offset
self.node_matrix[num]['pos'][1] = self.node_matrix[num]['pos'][1] - cos(yaw)*self.offset
if 'base_footprint' in cf_id:
self.current_pos,self.current_quat=self.tf.lookupTransform("/map", "/base_link", self.tf.getLatestCommonTime("/map", "/base_link"))
def goto(self, pos, quat):
# Send a goal
self.goal_sent = True
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))
# Start moving
self.move_base.send_goal(goal)
# Allow TurtleBot up to 60 seconds to complete task
success = self.move_base.wait_for_result(rospy.Duration(60))
state = self.move_base.get_state()
result = False
if success and state == GoalStatus.SUCCEEDED:
# We made it!
result = True
else:
self.move_base.cancel_goal()
self.goal_sent = False
return result
# listen (adapted from line_follower
def processSensing(BumperEvent):
global bump
bump = BumperEvent.bumper
print bump
#newInfo = True
def shutdown(self):
if self.goal_sent:
self.move_base.cancel_goal()
rospy.loginfo("Stop")
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 TestMotionExecutionBuffer(unittest.TestCase):
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def tearDown(self):
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "all";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.REMOVE
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05#rospy.get_param(padd_name)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
for z in range(2):
min_dist = 1000
if(z%2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime("/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint("base_link",finger_point)
distance = math.sqrt(math.pow(finger_point_base.point.x-.6,2)+math.pow(finger_point_base.point.y+.6,2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",finger_point_base.point.x,finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
rospy.loginfo("Min dist %d is %g",z,min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd-extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
class Bag2UosConverter:
def __init__(self, rootdir, scale_factor, transform_pitch_angle):
rospy.init_node('bad_to_uos_convert', anonymous=False)
rospy.Subscriber('velotime_points', PointCloud2, self.processMsg)
self.counter = 0
self.rootdir = rootdir
self.scale_factor = scale_factor
self.tf = TransformListener()
self.init = True
self.transform_pitch_angle = math.radians(transform_pitch_angle)
pass
def transformPoint(self,x ,y , z):
yy = y
xx = math.cos(self.transform_pitch_angle) * x + math.sin(self.transform_pitch_angle) * z
zz = -math.sin(self.transform_pitch_angle) * x + math.cos(self.transform_pitch_angle) * z
return xx, yy, zz
def run(self):
rospy.spin()
pass
def processMsg(self, msg):
print 'data arrived'
# print list(cloud)
# print self.tf.frameExists("velodyne") , self.tf.frameExists("odom")
if self.tf.frameExists("world") and self.tf.frameExists("odom"):
t = self.tf.getLatestCommonTime("world", "odom")
position, quat= self.tf.lookupTransform("world", "odom", t)
print position, quat
euler = tf.transformations.euler_from_quaternion(quat)
# heading_rad = math.degrees(euler[2])
heading_rad = euler[2]
print heading_rad
cloud = pc2.read_points(msg, skip_nans=True)
self.saveData(list(cloud))
self.savePose(position[0],position[1],0,0,0,heading_rad)
self.counter = self.counter + 1
if self.init:
self.init = False
cloud = pc2.read_points(msg, skip_nans=True)
self.saveData(list(cloud))
self.savePose(0,0,0,0,0,0)
self.counter = self.counter + 1
def saveData(self, cloud):
file_string = 'scan'+format(self.counter,'03d')
fullfilename_data = self.rootdir + file_string + '.3d'
fh_data = open(fullfilename_data, 'w')
for (x,y,z,intensity,ring) in cloud:
# print x,y,z
x, y, z = self.transformPoint(x, y, z)
x = x*self.scale_factor
y = y*self.scale_factor
z = z*self.scale_factor
str_ = format(x, '.1f') + ' ' + format(z, '.1f') + ' ' + format(y, '.1f') + '\n'
fh_data.write(str_)
fh_data.close()
def savePose(self, x, y, z, roll, pitch, yaw):
file_string = 'scan'+format(self.counter,'03d')
fullfilename_pose = self.rootdir + file_string + '.pose'
fh_pose = open(fullfilename_pose, 'w')
x = x*self.scale_factor
y = y*self.scale_factor
z = z*self.scale_factor
roll = roll*180/math.pi
yaw = yaw*180/math.pi
pitch = pitch*180/math.pi
str_first_line = format(x, '.1f') + ' ' + format(-z, '.1f') + ' ' + format(y, '.1f') + '\n'
str_2nd_line = format(roll, '.1f') + ' ' + format(-yaw, '.1f') + ' ' + format(pitch, '.1f') + '\n'
print str_first_line+str_2nd_line
fh_pose.writelines([str_first_line, str_2nd_line])
fh_pose.close()
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
Land = 3
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, -20, 20, "x")
self.pidY = PID(-20, -12, 0.0, -20, 20, "y")
#50000 800
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw")
self.state = Controller.Manual
#Target Values
self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1)
self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1)
self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1)
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.5
self.des_angle = 0.0
#self.power = 50000.0
#Actual Values
self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1)
self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1)
self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1)
self.lastJoy = Joy()
#Path view
self.pubPath = rospy.Publisher('cf_Uni_path',
MarkerArray,
queue_size=100)
self.path = MarkerArray()
#self.p = []
#Square trajectory
self.square_start = False
self.square_pos = 0
#self.square =[[0.5,0.5,0.5,0.0],
# [0.5,-0.5,0.5,90.0],
# [-0.5,-0.5,0.5,180.0],
# [-0.5,0.5,0.5,270.0]]
#landing flag
self.land_flag = False
self.power = 0.0
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 square_go(self):
if self.square_start == False:
self.square_pos = 0
self.targetX = square[self.square_pos][0]
self.targetY = square[self.square_pos][1]
self.targetZ = square[self.square_pos][2]
self.des_angle = square[self.square_pos][3]
self.square_pos = self.square_pos + 1
self.square_start = True
else:
self.targetX = square[self.square_pos][0]
self.targetY = square[self.square_pos][1]
self.targetZ = square[self.square_pos][2]
self.des_angle = square[self.square_pos][3]
self.square_pos = self.square_pos + 1
if self.square_pos == 4:
self.square_pos = 0
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!")
self.land_flag = False
#thrust = self.cmd_vel_telop.linear.z
#print(thrust)
self.pidReset()
self.pidZ.integral = 40.0
#self.targetZ = 1
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.land_flag = False
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.land_flag = False
self.state = Controller.TakeOff
print("TakeOff!")
#Button 4
if delta[3] == 1:
self.land_flag = True
print("Landing!")
self.square_start = False
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.4
self.des_angle = 0.0
self.state = Controller.Automatic
#Button 5
if delta[4] == 1:
self.square_go()
#self.targetX = square[0][0]
#self.targetY = square[0][1]
#self.targetZ = square[0][2]
#self.des_angle = square[0][3]
#print(self.targetZ)
#self.power += 100.0
#print(self.power)
self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
self.square_start = False
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.5
self.des_angle = 0.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_Mark_Gon4")
print(
t,
self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t))
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", t)
print(position[0], position[1], position[2])
#if position[2] > 2.0 or thrust > 54000:
if thrust > 55000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 500
#self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Land:
t = self.listener.getLatestCommonTime("/mocap",
"/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", t)
if position[2] > 0.05:
msg_land = self.cmd_vel_telop
self.power -= 100
msg_land.linear.z = self.power
self.pubNav.publish(msg_land)
else:
msg_land = self.cmd_vel_telop
msg_land.linear.z = 0
self.pubNav.publish(msg_land)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap",
"/Nano_Mark_Gon4")
print(t)
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform(
"/mocap", "/Nano_Mark_Gon4", 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)
#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(position[2],self.targetZ)
#print(z_prim)
#if z_prim < self.power:
# msg.linear.z = self.power
#else:
# msg.linear.z = z_prim
#msg.linear.z = self.power
#print(self.power)
msg.linear.z = self.pidZ.update(
0.0, self.targetZ - position[2]
) #self.pidZ.update(position[2], self.targetZ)
msg.angular.z = self.pidYaw.update(
0.0,
self.des_angle * (math.pi / 180) +
euler[2]) #*(180/math.pi))
#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)
#Publish Real and Target position
self.pubtarX.publish(self.targetX)
self.pubtarY.publish(self.targetY)
self.pubtarZ.publish(self.targetZ)
self.pubrealX.publish(position[0])
self.pubrealY.publish(position[1])
self.pubrealZ.publish(position[2])
#change square point
if abs(self.targetX-position[0])<0.08 and \
abs(self.targetY-position[1])<0.08 and \
abs(self.targetZ-position[2])<0.08 and \
self.square_start == True:
self.square_go()
#Landing
if abs(self.targetX-position[0])<0.1 and \
abs(self.targetY-position[1])<0.1 and \
abs(self.targetZ-position[2])<0.1 and \
self.land_flag == True:
self.state = Controller.Land
self.power = msg.linear.z
#Publish Path
#point = Marker()
#line = Marker()
#point.header.frame_id = line.header.frame_id = 'mocap'
#POINTS
#point.action = point.ADD
#point.pose.orientation.w = 1.0
#point.id = 0
#point.type = point.POINTS
#point.scale.x = 0.01
#point.scale.y = 0.01
#point.color.g = 1.0
#point.color.a = 1.0
#LINE
#line.action = line.ADD
#line.pose.orientation.w = 1.0
#line.id = 1
#line.type = line.LINE_STRIP
#line.scale.x = 0.01
#line.color.g = 1.0
#line.color.a = 1.0
#p = Point()
#p.x = position[0]
#p.y = position[1]
#p.z = position[2]
#point.points.append(p)
# line.points.append(p)
#self.path.markers.append(p)
#id = 0
#for m in self.path.markers:
# m.id = id
# id += 1
#self.pubPath.publish(self.path)
#self.pubPath.publish(point)
#self.pubPath.publish(line)
point = Marker()
point.header.frame_id = 'mocap'
point.type = point.SPHERE
#points.header.stamp = rospy.Time.now()
point.ns = 'cf_Uni_path'
point.action = point.ADD
#points.id = 0;
point.scale.x = 0.005
point.scale.y = 0.005
point.scale.z = 0.005
point.color.a = 1.0
point.color.r = 1.0
point.color.g = 1.0
point.color.b = 0.0
point.pose.orientation.w = 1.0
point.pose.position.x = position[0]
point.pose.position.y = position[1]
point.pose.position.z = position[2]
self.path.markers.append(point)
id = 0
for m in self.path.markers:
m.id = id
id += 1
self.pubPath.publish(self.path)
#point = Point()
#point.x = position[0]
#point.y = position[1]
#point.z = position[2]
#points.points.append(point)
#self.p.append(pos2path)
#self.path.header.stamp = rospy.Time.now()
#self.path.header.frame_id = 'mocap'
#self.path.poses = self.p
#self.pubPath.publish(points)
rospy.sleep(0.01)
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 ParticleFilter:
""" The class that represents a Particle Filter ROS Node
Attributes list:
initialized: a Boolean flag to communicate to other class methods that initializaiton is complete
base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots)
map_frame: the name of the map coordinate frame (should be "map" in most cases)
odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases)
scan_topic: the name of the scan topic to listen to (should be "scan" in most cases)
n_particles: the number of particles in the filter
linear_mov: the amount of linear movement before triggering a filter update
angular_mov: the amount of angular movement before triggering a filter update
laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation
pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI)
particle_pub: a publisher for the particle cloud
laser_subscriber: listens for new scan data on topic self.scan_topic
tf_listener: listener for coordinate transforms
particle_cloud: a list of particles representing a probability distribution over robot poses
current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed.
The pose is expressed as a list [x,y,theta] (where theta is the yaw)
map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid
"""
def __init__(self):
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('RMI_pf')
self.base_frame = "base_link" # the frame of the robot base
self.map_frame = "map" # the name of the map coordinate frame
self.odom_frame = "odom" # the name of the odometry coordinate frame
self.scan_topic = "scan" # the topic where we will get laser scans from
self.n_particles = 20
self.linear_mov = 0.1
self.angular_mov = math.pi / 10
self.laser_max_distance = 2.0
self.sigma = 0.05
# Descomentar essa linha caso /initialpose seja publicada
# self.pose_listener = rospy.Subscriber("initialpose",
# PoseWithCovarianceStamped,
# self.update_initial_pose)
self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan,
self.scan_received)
self.particle_pub = rospy.Publisher("particlecloud_rmi",
PoseArray,
queue_size=1)
self.tf_listener = TransformListener()
self.particle_cloud = []
self.current_odom_xy_theta = []
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
self.occupancy_field = OccupancyField(self.map)
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(1.0))
self.initialized = True
def update_particles_with_odom(self, msg):
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# print 'new_odom_xy_theta', new_odom_xy_theta
# Pega a posicao da odom (x,y,tehta)
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
# print 'delta', delta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for particle in self.particle_cloud:
d = math.sqrt((delta[0]**2) + (delta[1]**2))
# print 'particle_theta_1', particle.theta
particle.x += d * (math.cos(particle.theta) + normal(0, 0.01))
particle.y += d * (math.sin(particle.theta) + normal(0, 0.01))
particle.theta = self.current_odom_xy_theta[2] #+ normal(0,0.05)
# Systematic Resample
def resample_particles(self):
self.normalize_particles()
# for particle in self.particle_cloud:
# print 'TODAS PART', particle.w, particle.x, particle.y
weights = []
for particle in self.particle_cloud:
weights.append(particle.w)
newParticles = []
N = len(weights)
positions = (np.arange(N) + random.random()) / N
cumulative_sum = np.cumsum(weights)
i, j = 0, 0
while i < N:
if positions[i] < cumulative_sum[j]:
newParticles.append(deepcopy(self.particle_cloud[j]))
i += 1
else:
j += 1
self.particle_cloud = newParticles
def update_particles_with_laser(self, msg):
depths = []
for dist in msg.ranges:
if not np.isnan(dist):
depths.append(dist)
fullDepthsArray = msg.ranges[:]
# Verifica se ha objetos proximos ao robot
if len(depths) == 0:
self.closest = 0
self.position = 0
else:
self.closest = min(depths)
self.position = fullDepthsArray.index(self.closest)
# print 'self.position, self.closest', self.position, self.closest, self.xy_theta_aux
# print msg, '/scan'
for index, particle in enumerate(self.particle_cloud):
tot_prob = 0.0
for index, scan in enumerate(depths):
x, y = self.transform_scan(particle, scan, index)
# print 'x,y, scan', x, y, scan
# usa o metodo get_closest_obstacle_distance para buscar o obstaculo mais proximo dentro do range x,y do grid map
d = self.occupancy_field.get_closest_obstacle_distance(x, y)
# quanto mais proximo de zero mais relevante
tot_prob += math.exp((-d**2) / (2 * self.sigma**2))
tot_prob = tot_prob / len(depths)
if math.isnan(tot_prob):
particle.w = 1.0
else:
particle.w = tot_prob
# print 'LASER', particle.x, particle.y, particle.w
def transform_scan(self, particle, distance, theta):
return (particle.x +
distance * math.cos(math.radians(particle.theta + theta)),
particle.y +
distance * math.sin(math.radians(particle.theta + theta)))
def update_initial_pose(self, msg):
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
def initialize_particle_cloud(self, xy_theta=None):
print 'Cria o set inicial de particulas'
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
x, y, theta = xy_theta
# Altere este parametro para aumentar a circunferencia do filtro de particulas
# Na VM ate 1 e suportado
rad = 0.5
self.particle_cloud = []
self.particle_cloud.append(
Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
# print 'particle_values_W', self.particle_cloud[0].w
# print 'particle_values_X', self.particle_cloud[0].x
# print 'particle_values_Y', self.particle_cloud[0].y
# print 'particle_values_THETA', self.particle_cloud[0].theta
for i in range(self.n_particles - 1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x, y, theta)
self.particle_cloud.append(particle)
self.normalize_particles()
def normalize_particles(self):
tot_weight = sum([particle.w
for particle in self.particle_cloud]) or 1.0
for particle in self.particle_cloud:
particle.w = particle.w / tot_weight
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose(
)) # transforma a particula em POSE para ser entendida pelo ROS
# print 'PARTII', [particles.x for particles in self.particle_cloud]
# Publica as particulas no rviz (particloud_rmi)
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
# print msg
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, msg.header.frame_id, msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame,
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# print 'msg.header.frame_id', msg.header.frame_id
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
# listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
# p = PoseStamped(header=Header(stamp=msg.header.stamp,
p = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.base_frame, self.map_frame),
# p = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.base_frame),
pose=Pose())
# p_aux = PoseStamped(header=Header(stamp=self.tf_listener.getLatestCommonTime("/base_link","/map"),
p_aux = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.odom_frame, self.map_frame),
# p_aux = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.odom_frame),
pose=Pose())
odom_aux = self.tf_listener.transformPose(self.map_frame, p_aux)
odom_aux_xy_theta = convert_pose_to_xy_and_theta(odom_aux.pose)
# print 'odom_aux_xy_theta', odom_aux_xy_theta
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# print 'self.odom_pose', self.odom_pose
# (trans, root) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
# self.odom_pose = trans
# print trans, root
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# new_odom_xy_theta = convert_pose_to_xy_and_theta(self.laser_pose.pose)
xy_theta_aux = (new_odom_xy_theta[0] + odom_aux_xy_theta[0],
new_odom_xy_theta[1] + odom_aux_xy_theta[1],
new_odom_xy_theta[2])
self.xy_theta_aux = xy_theta_aux
if not (self.particle_cloud):
self.initialize_particle_cloud(xy_theta_aux)
self.current_odom_xy_theta = new_odom_xy_theta
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.linear_mov
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.linear_mov
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.angular_mov):
self.update_particles_with_odom(msg)
self.update_particles_with_laser(msg)
self.resample_particles()
self.publish_particles(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
'(see description, default: 0.3)', default=0.3,
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
class Controller():
Manual = 0
Automatic = 1
TakeOff = 2
Land = 3
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, -20, 20, "x")
self.pidY = PID(-20, -12, 0.0, -20, 20, "y")
#50000 800
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw")
self.state = Controller.Manual
#Target Values
self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1)
self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1)
self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1)
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.5
self.des_angle = 0.0
#self.power = 50000.0
#Actual Values
self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1)
self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1)
self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1)
self.lastJoy = Joy()
#Path view
self.pubPath = rospy.Publisher('cf_Uni_path', MarkerArray, queue_size=100)
self.path = MarkerArray()
#self.p = []
#Square trajectory
self.square_start = False
self.square_pos = 0
#self.square =[[0.5,0.5,0.5,0.0],
# [0.5,-0.5,0.5,90.0],
# [-0.5,-0.5,0.5,180.0],
# [-0.5,0.5,0.5,270.0]]
#landing flag
self.land_flag = False
self.power = 0.0
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 square_go(self):
if self.square_start == False:
self.square_pos = 0
self.targetX = square[self.square_pos][0]
self.targetY = square[self.square_pos][1]
self.targetZ = square[self.square_pos][2]
self.des_angle = square[self.square_pos][3]
self.square_pos = self.square_pos + 1
self.square_start = True
else:
self.targetX = square[self.square_pos][0]
self.targetY = square[self.square_pos][1]
self.targetZ = square[self.square_pos][2]
self.des_angle = square[self.square_pos][3]
self.square_pos = self.square_pos + 1
if self.square_pos == 4:
self.square_pos = 0
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!")
self.land_flag = False
#thrust = self.cmd_vel_telop.linear.z
#print(thrust)
self.pidReset()
self.pidZ.integral = 40.0
#self.targetZ = 1
self.state = Controller.Automatic
#Button 2
if delta[1] == 1 and self.state != Controller.Manual:
print("Manual!")
self.land_flag = False
self.pubNav.publish(self.cmd_vel_telop)
self.state = Controller.Manual
#Button 3
if delta[2] == 1:
self.land_flag = False
self.state = Controller.TakeOff
print("TakeOff!")
#Button 4
if delta[3] == 1:
self.land_flag = True
print("Landing!")
self.square_start = False
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.4
self.des_angle = 0.0
self.state = Controller.Automatic
#Button 5
if delta[4] == 1:
self.square_go()
#self.targetX = square[0][0]
#self.targetY = square[0][1]
#self.targetZ = square[0][2]
#self.des_angle = square[0][3]
#print(self.targetZ)
#self.power += 100.0
#print(self.power)
self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
self.square_start = False
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.5
self.des_angle = 0.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_Mark_Gon4")
print(t,self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t))
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
print(position[0],position[1],position[2])
#if position[2] > 2.0 or thrust > 54000:
if thrust > 55000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 500
#self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Land:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
if position[2] > 0.05:
msg_land = self.cmd_vel_telop
self.power -=100
msg_land.linear.z = self.power
self.pubNav.publish(msg_land)
else:
msg_land = self.cmd_vel_telop
msg_land.linear.z = 0
self.pubNav.publish(msg_land)
if self.state == Controller.Automatic:
# transform target world coordinates into local coordinates
t = self.listener.getLatestCommonTime("/mocap","/Nano_Mark_Gon4")
print(t)
if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",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)
#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(position[2],self.targetZ)
#print(z_prim)
#if z_prim < self.power:
# msg.linear.z = self.power
#else:
# msg.linear.z = z_prim
#msg.linear.z = self.power
#print(self.power)
msg.linear.z = self.pidZ.update(0.0,self.targetZ-position[2]) #self.pidZ.update(position[2], self.targetZ)
msg.angular.z = self.pidYaw.update(0.0,self.des_angle*(math.pi/180) + euler[2])#*(180/math.pi))
#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)
#Publish Real and Target position
self.pubtarX.publish(self.targetX)
self.pubtarY.publish(self.targetY)
self.pubtarZ.publish(self.targetZ)
self.pubrealX.publish(position[0])
self.pubrealY.publish(position[1])
self.pubrealZ.publish(position[2])
#change square point
if abs(self.targetX-position[0])<0.08 and \
abs(self.targetY-position[1])<0.08 and \
abs(self.targetZ-position[2])<0.08 and \
self.square_start == True:
self.square_go()
#Landing
if abs(self.targetX-position[0])<0.1 and \
abs(self.targetY-position[1])<0.1 and \
abs(self.targetZ-position[2])<0.1 and \
self.land_flag == True:
self.state = Controller.Land
self.power = msg.linear.z
#Publish Path
#point = Marker()
#line = Marker()
#point.header.frame_id = line.header.frame_id = 'mocap'
#POINTS
#point.action = point.ADD
#point.pose.orientation.w = 1.0
#point.id = 0
#point.type = point.POINTS
#point.scale.x = 0.01
#point.scale.y = 0.01
#point.color.g = 1.0
#point.color.a = 1.0
#LINE
#line.action = line.ADD
#line.pose.orientation.w = 1.0
#line.id = 1
#line.type = line.LINE_STRIP
#line.scale.x = 0.01
#line.color.g = 1.0
#line.color.a = 1.0
#p = Point()
#p.x = position[0]
#p.y = position[1]
#p.z = position[2]
#point.points.append(p)
# line.points.append(p)
#self.path.markers.append(p)
#id = 0
#for m in self.path.markers:
# m.id = id
# id += 1
#self.pubPath.publish(self.path)
#self.pubPath.publish(point)
#self.pubPath.publish(line)
point = Marker()
point.header.frame_id = 'mocap'
point.type = point.SPHERE
#points.header.stamp = rospy.Time.now()
point.ns = 'cf_Uni_path'
point.action = point.ADD
#points.id = 0;
point.scale.x = 0.005
point.scale.y = 0.005
point.scale.z = 0.005
point.color.a = 1.0
point.color.r = 1.0
point.color.g = 1.0
point.color.b = 0.0
point.pose.orientation.w = 1.0
point.pose.position.x = position[0]
point.pose.position.y = position[1]
point.pose.position.z = position[2]
self.path.markers.append(point)
id = 0
for m in self.path.markers:
m.id = id
id += 1
self.pubPath.publish(self.path)
#point = Point()
#point.x = position[0]
#point.y = position[1]
#point.z = position[2]
#points.points.append(point)
#self.p.append(pos2path)
#self.path.header.stamp = rospy.Time.now()
#self.path.header.frame_id = 'mocap'
#self.path.poses = self.p
#self.pubPath.publish(points)
rospy.sleep(0.01)
joint_dim = SENSOR_DIMS[JOINT_ANGLES] + SENSOR_DIMS[JOINT_VELOCITIES]
# Initialized to start position and inital velocities are 0
x0 = np.zeros(state_space)
x0[:SENSOR_DIMS[JOINT_ANGLES]] = ja_x0
# Need for this node will go away upon migration to KDL
rospy.init_node('gps_agent_ur_ros_node')
# Set starting end effector position using TF
tf = TransformListener()
# Sleep for .1 secs to give the node a chance to kick off
rospy.sleep(1)
time = tf.getLatestCommonTime(WRIST_3_LINK, BASE_LINK)
x0[joint_dim:(joint_dim +
NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position(
tf, WRIST_3_LINK, BASE_LINK, time)
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(
get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
reset_condition = {JOINT_ANGLES: INITIAL_JOINTS, JOINT_VELOCITIES: []}
x0s.append(x0)
ee_tgts.append(ee_tgt)
reset_conditions.append(reset_condition)
class ArmByFtWrist(object):
def __init__(self):
rospy.loginfo("Initializing...")
# Node Hz rate
self.rate = 10
self.rate_changed = False
self.send_goals = False
# Limits
self.min_x = 0.0
self.max_x = 0.6
self.min_y = -0.5
self.max_y = -0.05
self.min_z = -0.2
self.max_z = 0.2
# Force stuff
self.fx_scaling = 0.0
self.fy_scaling = 0.0
self.fz_scaling = 0.0
self.fx_deadband = 0.0
self.fy_deadband = 0.0
self.fz_deadband = 0.0
# Torque stuff
self.tx_scaling = 0.0
self.ty_scaling = 0.0
self.tz_scaling = 0.0
self.tx_deadband = 0.0
self.ty_deadband = 0.0
self.tz_deadband = 0.0
self.dyn_rec_srv = Server(HandshakeConfig, self.dyn_rec_callback)
# Signs adjusting fx, fy, fz, tx(roll), ty(pitch), tz(yaw)
# for the left hand of reemc right now
# And axis flipping
self.frame_fixer = WrenchFixer(1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
'x', 'y', 'z',
'x', 'y', 'z')
# Goal to send to WBC
self.tf_l = TransformListener()
rospy.sleep(3.0) # Let the subscriber to its job
self.initial_pose = self.get_initial_pose()
self.current_pose = self.initial_pose
self.last_pose_to_follow = deepcopy(self.current_pose)
self.pose_pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_tool_link_goal',
PoseStamped,
queue_size=1)
# Safe debugging goal
self.debug_pose_pub = rospy.Publisher('/force_torqued_pose',
PoseStamped,
queue_size=1)
# Goal to follow
self.follow_sub = rospy.Subscriber('/pose_to_follow',
PoseStamped,
self.follow_pose_cb,
queue_size=1)
# Sensor input
self.last_wrench = None
self.wrench_sub = rospy.Subscriber('/right_wrist_ft',
WrenchStamped,
self.wrench_cb,
queue_size=1)
rospy.loginfo("Done initializing.")
def dyn_rec_callback(self, config, level):
"""
:param config:
:param level:
:return:
"""
rospy.loginfo("Received reconf call: " + str(config))
# Node Hz rate
if self.rate != config['rate']:
self.rate_changed = True
self.rate = config['rate']
self.send_goals = config['send_goals']
# Limits
self.min_x = config['min_x']
self.max_x = config['max_x']
self.min_y = config['min_y']
self.max_y = config['max_y']
self.min_z = config['min_z']
self.max_z = config['max_z']
# Force stuff
self.fx_scaling = config['fx_scaling']
self.fy_scaling = config['fy_scaling']
self.fz_scaling = config['fz_scaling']
self.fx_deadband = config['fx_deadband']
self.fy_deadband = config['fy_deadband']
self.fz_deadband = config['fz_deadband']
# Torque stuff
self.tx_scaling = config['tx_scaling']
self.ty_scaling = config['ty_scaling']
self.tz_scaling = config['tz_scaling']
self.tx_deadband = config['tx_deadband']
self.ty_deadband = config['ty_deadband']
self.tz_deadband = config['tz_deadband']
return config
def follow_pose_cb(self, data):
if data.header.frame_id != '/world':
rospy.logwarn(
"Poses to follow should be in frame /world, transforming into /world...")
world_ps = self.transform_pose_to_world(
data.pose, from_frame=data.header.frame_id)
ps = PoseStamped()
ps.header.stamp = data.header.stamp
ps.header.frame_id = '/world'
ps.pose = world_ps
self.last_pose_to_follow = ps
else:
self.last_pose_to_follow = data
def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"):
ps = PoseStamped()
ps.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
ps.header.frame_id = "/" + from_frame
# TODO: check pose being PoseStamped or Pose
ps.pose = pose
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_ps = self.tf_l.transformPose("/world", ps)
transform_ok = True
return world_ps
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
ps.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def wrench_cb(self, data):
self.last_wrench = data
def get_initial_pose(self):
zero_pose = Pose()
zero_pose.orientation.w = 1.0
current_pose = self.transform_pose_to_world(
zero_pose, from_frame="wrist_right_ft_link")
return current_pose
def get_abs_total_force(self, wrench_msg):
f = wrench_msg.wrench.force
return abs(f.x) + abs(f.y) + abs(f.z)
def get_abs_total_torque(self, wrench_msg):
t = wrench_msg.wrench.torque
return abs(t.x) + abs(t.y) + abs(t.z)
def run(self):
while not rospy.is_shutdown() and self.last_wrench is None:
rospy.sleep(0.2)
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
# To change the node rate
if self.rate_changed:
r = rospy.Rate(self.rate)
self.rate_changed = False
self.move_towards_force_torque_with_tf()
r.sleep()
def move_towards_force_torque_with_tf(self):
fx, fy, fz = self.get_force_movement()
rospy.loginfo(
"fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3))))
send_new_goal = False
ps = Pose()
if abs(fx) > self.fx_deadband:
ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx
send_new_goal = True
if abs(fy) > self.fy_deadband:
ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy
send_new_goal = True
if abs(fz) > self.fz_deadband:
ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz
send_new_goal = True
tx, ty, tz = self.get_torque_movement()
rospy.loginfo(
"tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3))))
ori_change = False
roll = pitch = yaw = 0.0
if abs(tx) > self.tx_deadband:
roll += (tx * self.tx_scaling) * self.frame_fixer.tx
send_new_goal = True
ori_change = True
if abs(ty) > self.ty_deadband:
pitch += (ty * self.ty_scaling) * self.frame_fixer.ty
send_new_goal = True
ori_change = True
if abs(tz) > self.tz_deadband:
yaw += (tz * self.tz_scaling) * self.frame_fixer.tz
send_new_goal = True
ori_change = True
# if not send_new_goal:
# rospy.loginfo("Not moving because of force torque.")
# return
q = quaternion_from_euler(roll, pitch, yaw)
ps.orientation = Quaternion(*q)
# rospy.loginfo("Local pose modification: " + str(ps))
# this pose is the current wrist link
# plus the modification of the scalings
force_torqued_pose = self.transform_pose_to_world(
ps, from_frame="wrist_right_ft_link")
#rospy.loginfo("Force-torque'd pose: " + str(force_torqued_pose))
# Now we get the difference with the wrist_right_ft_link (this should be optimized)
# And we add that to the last goal pose
wps = Pose()
wps.orientation.w = 1.0
wrist_right_ft_link_pose = self.transform_pose_to_world(wps,
from_frame="wrist_right_ft_link")
x_diff = force_torqued_pose.pose.position.x - wrist_right_ft_link_pose.pose.position.x
y_diff = force_torqued_pose.pose.position.y - wrist_right_ft_link_pose.pose.position.y
z_diff = force_torqued_pose.pose.position.z - wrist_right_ft_link_pose.pose.position.z
self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + x_diff
self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + y_diff
self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + z_diff
ori_change = True
if ori_change:
# force torque RPY
o = force_torqued_pose.pose.orientation
r_ft, p_ft, y_ft = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("ForceTorqued ori: " + str((round(r_ft, 3), round(p_ft, 3), round(y_ft, 3))))
# wrist ft link RPY
o2 = wrist_right_ft_link_pose.pose.orientation
r_w, p_w, y_w = euler_from_quaternion([o2.x, o2.y, o2.z, o2.w])
rospy.loginfo("WristIdentity ori: " + str((round(r_w, 3), round(p_w, 3), round(y_w, 3))))
# last pose to follow RPY
o3 = self.last_pose_to_follow.pose.orientation
r_lp, p_lp, y_lp = euler_from_quaternion([o3.x, o3.y, o3.z, o3.w])
rospy.loginfo("Lastpose ori: " + str((round(r_lp, 3), round(p_lp, 3), round(y_lp, 3))))
roll_diff = r_ft - r_w
pitch_diff = p_ft - p_w
yaw_diff = y_ft - y_w
rospy.loginfo("Diffs ori: " + str((round(roll_diff, 3), round(pitch_diff, 3), round(yaw_diff, 3))))
# Substract the constant orientation transform from tool_link
zero_tool_link = self.transform_pose_to_world(
ps, from_frame="arm_right_tool_link")
o4 = zero_tool_link.pose.orientation
r_tl, p_tl, y_tl = euler_from_quaternion([o4.x, o4.y, o4.z, o4.w])
rospy.loginfo("tool link ori: " + str((round(r_tl, 3), round(p_tl, 3), round(y_tl, 3))))
final_roll = r_tl - roll_diff
final_pitch = p_tl - pitch_diff
final_yaw = y_tl - yaw_diff
q2 = quaternion_from_euler(final_roll, final_pitch, final_yaw)
self.current_pose.pose.orientation = Quaternion(*q2)
# else:
# rospy.loginfo("No change in ori")
# self.current_pose.pose.orientation = self.last_pose_to_follow.pose.orientation # debug
self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x,
self.min_x,
self.max_x)
self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y,
self.min_y,
self.max_y)
self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z,
self.min_z,
self.max_z)
#rospy.loginfo("Workspace pose:\n" + str(self.current_pose.pose))
if self.send_goals and send_new_goal:
self.pose_pub.publish(self.current_pose)
self.debug_pose_pub.publish(self.current_pose)
def get_force_movement(self):
f_x = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.x_axis)
f_y = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.y_axis)
f_z = self.last_wrench.wrench.force.__getattribute__(
self.frame_fixer.z_axis)
return f_x, f_y, f_z
def get_torque_movement(self):
t_x = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.roll_axis)
t_y = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.pitch_axis)
t_z = self.last_wrench.wrench.torque.__getattribute__(
self.frame_fixer.yaw_axis)
return t_x, t_y, t_z
def sanitize(self, data, min_v, max_v):
if data > max_v:
return max_v
if data < min_v:
return min_v
return data
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, -20, 20, "x")
self.pidY = PID(-20, -12, 0.0, -20, 20, "y")
#50000 800
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw")
self.state = Controller.Manual
#Target Values
self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1)
self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1)
self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1)
self.targetZ = 0.5
self.targetX = 0.0
self.targetY = 0.0
self.des_angle = 0.0
#self.power = 50000.0
#Actual Values
self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1)
self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1)
self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1)
self.lastJoy = Joy()
#Path
self.pubPath = rospy.Publisher('cf_Gon_path', Path, queue_size=1)
self.path = Path()
self.p = []
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 = 40.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.targetX = -1.0
#self.targetY = -1.0
self.targetZ = 1.0
self.des_angle = -90.0
#if self.des_angle > 179:
# self.des_angle = -179.0
#print(self.targetZ)
#self.power += 100.0
#print(self.power)
self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
#self.targetX = 1.0
#self.targetY = 1.0
self.targetZ = 0.5
self.des_angle = 90.0
#if self.des_angle < -179:
# self.des_angle = 179.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_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
print(position[0],position[1],position[2])
#if position[2] > 0.1 or thrust > 50000:
if thrust > 51000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 300
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_Mark_Gon4")
print(t)
if self.listener.canTransform("/mocap","/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4",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)
#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(position[2],self.targetZ)
#print(z_prim)
#if z_prim < self.power:
# msg.linear.z = self.power
#else:
# msg.linear.z = z_prim
#msg.linear.z = self.power
#print(self.power)
msg.linear.z = self.pidZ.update(0.0,self.targetZ-position[2]) #self.pidZ.update(position[2], self.targetZ)
msg.angular.z = self.pidYaw.update(0.0,self.des_angle*(math.pi/180) + euler[2])#*(180/math.pi))
#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)
#Publish Real and Target position
self.pubtarX.publish(self.targetX)
self.pubtarY.publish(self.targetY)
self.pubtarZ.publish(self.targetZ)
self.pubrealX.publish(position[0])
self.pubrealY.publish(position[1])
self.pubrealZ.publish(position[2])
print("que pasaaa")
#Publish Path
pos2path = PoseStamped()
pos2path.pose.position.x = position[0]
pos2path.pose.position.y = position[1]
pos2path.pose.position.z = position[2]
pos2path.pose.orientation.x = quaternion[0]
pos2path.pose.orientation.y = quaternion[1]
pos2path.pose.orientation.z = quaternion[2]
pos2path.pose.orientation.w = quaternion[3]
self.p.append(pos2path)
print('holaaaaa')
self.path.header.frame_id = 'mocap'
self.path.poses = self.p
self.pubPath.publish(self.path)
rospy.sleep(0.01)
class calib:
def __init__(self, *args):
self.tf = TransformListener()
self.detector_service = rospy.ServiceProxy("/fiducials/get_fiducials", DetectObjects)
self.frame_camera_mount = rospy.get_param('~frame_camera_mount')
self.frame_marker_mount = rospy.get_param('~frame_marker_mount')
self.frame_marker = rospy.get_param('~frame_marker', "/marker")
def compute(self):
x_values = []
y_values = []
z_values = []
roll_values = []
pitch_values = []
yaw_values = []
count_success = 0
count_failed = 0
while count_success <= 15 and count_failed <= 100:
try:
rospy.wait_for_service("/fiducials/get_fiducials", 3.0)
res = self.detector_service(DetectObjectsRequest())
if self.tf.frameExists(self.frame_camera_mount) and self.tf.frameExists(self.frame_marker_mount):
t = self.tf.getLatestCommonTime(self.frame_camera_mount, self.frame_marker_mount)
position, quaternion = self.tf.lookupTransform(self.frame_camera_mount, self.frame_marker_mount, t)
euler = tf.transformations.euler_from_quaternion(quaternion)
print '<origin xyz="'+str(position[0])+" "+str(position[1])+" "+str(position[2])+'" rpy="'+str(euler[0])+" "+str(euler[1])+" "+str(euler[2])+'" />'
x_values.append(position[0])
y_values.append(position[1])
z_values.append(position[2])
roll_values.append(euler[0])
pitch_values.append(euler[1])
yaw_values.append(euler[2])
else:
print "tf does not exist!", self.tf.frameExists(self.frame_camera_mount), self.tf.frameExists(self.frame_marker_mount)
except:
print "did not detect marker."
print "count_success = ", count_success
print "count_failed = ", count_failed
count_failed += 1
continue
count_success += 1
if len(x_values) < 5:
print "to few detections, aborting"
return
x_avg_value = (float)(sum(x_values))/len(x_values)
y_avg_value = (float)(sum(y_values))/len(y_values)
z_avg_value = (float)(sum(z_values))/len(z_values)
roll_avg_value = (float)(sum(roll_values))/len(roll_values)
pitch_avg_value = (float)(sum(pitch_values))/len(pitch_values)
yaw_avg_value = (float)(sum(yaw_values))/len(yaw_values)
print "The average values (without hardcoding) <xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+ str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n'
# HACK set some DOf to fixed values
z_avg_value = 1.80
roll_avg_value = -3.1415926
pitch_avg_value = 0
yaw_avg_value = -3.1415926
print "The average values<xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+ str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n'
