def chooseBestCandidate(self):
# Wait for the availability of this service
rospy.wait_for_service("move_base/make_plan")
# Get a proxy to execute the service
make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)
self.bestCandidate = None
start = PoseStamped()
start.header.frame_id = "map"
start.pose = self.robot_pose
goal = PoseStamped()
goal.header.frame_id = "map"
tolerance = 0.0
maxUtility = 0.0
_lambda = 0.2
# Find the candidate with the shortest path
for eachCandidate in self.candidates.values():
# Execute service for each candidates
goal.pose = eachCandidate
plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
# Compute the length of the path
pathLength = self.computePathLength(plan_response.plan)
quantityInformation = self.quantityOfNewInformation(eachCandidate)
# Compute the utility of eachCandidate
utility = pathLength * math.exp(-_lambda * quantityInformation)
if utility > maxUtility:
maxUtility = utility
self.bestCandidate = eachCandidate
def chooseBestCandidate(self):
# Wait for the availability of this service
rospy.wait_for_service("move_base/make_plan")
# Get a proxy to execute the service
make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)
self.bestCandidate = None
shortestLength = 0
firstCandidate = True
start = PoseStamped()
start.header.frame_id = "map"
start.pose = self.robot_pose
goal = PoseStamped()
goal.header.frame_id = "map"
tolerance = 0.0
# Find the candidate with the shortest path
for eachCandidate in self.candidates.values():
# Execute service for each candidates
goal.pose = eachCandidate
plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
# Compute the length of the path
pathLength = self.computePathLength(plan_response.plan)
# Set the shortestPath for the first candidate
if firstCandidate:
shortestPath = pathLength
self.bestCandidate = eachCandidate
firstCandidate = False
# If this is the shortest path until now, we found a new bestCandidate
if pathLength < shortestLength:
self.bestCandidate = eachCandidate
shortestLength = pathLength
def nav_cost(self, p1, p2):
ps1 = PoseStamped()
ps1.header.frame_id = 'map'
ps1.pose = p1
ps2 = PoseStamped()
ps2.header.frame_id = 'map'
ps2.pose = p2
if (ps1,ps2) in self.nav_cost_list:
#rospy.loginfo("Retrieve nav cost from cache (p1,p2)")
return self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))]
if (ps2,ps1) in self.nav_cost_list:
#rospy.loginfo("Retrieve nav cost from cache (p2,p1)")
return self.nav_cost_dict[self.nav_cost_list.index((ps2,ps1))]
lin_diff = math.sqrt( math.pow(ps1.pose.position.x - ps2.pose.position.x, 2) +
math.pow(ps1.pose.position.y - ps2.pose.position.y, 2))
lin_cost = lin_diff / self.nav_lin_vel
cost = lin_cost
self.nav_cost_list.append((ps1,ps2))
self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))] = cost
return cost
def chooseBestCandidate(self):
# Wait for the availability of this service
rospy.wait_for_service("move_base/make_plan")
# Get a proxy to execute the service
make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)
self.bestCandidate = None
start = PoseStamped()
start.header.frame_id = "map"
start.pose = self.robot_pose
goal = PoseStamped()
goal.header.frame_id = "map"
tolerance = 0.0
# Evaluation of each candidates for each criteria used
c = [] # Find the candidate with the shortest path
for eachCandidate in self.candidates.values():
# Execute service for each candidates
goal.pose = eachCandidate
plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
# Compute the length of the path
distance = self.computePathLength(plan_response.plan)
# Compute new quantity of information criteria for the candidate
qi = self.quantityOfNewInformation(eachCandidate)
# We negated Distance because we want to minimize this criteria
c.append({"Distance": -distance, "QuantityOfInformation": qi * 100, "pose": eachCandidate})
# Suppresion of candidates that are not on Pareto front
filteredCandidates = PyMCDA.paretoFilter(c, self.criteria)
decision = PyMCDA.decision(filteredCandidates, self.criteria, self.weights, self.preferenceFunction)
if decision:
self.bestCandidate = decision["pose"]
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('custom_1')
table_id = 'table'
self.tid = self.pwh.name.index('table')
obstacle1_id = 'obstacle1'
self.o1id = self.pwh.name.index('wood_cube_5cm')
obstacle2_id = 'obstacle2'
self.o2id = self.pwh.name.index('custom_2')
# Set the sizes [l, w, h]
table_size = [1.5, 0.8, 0.03]
target_size = [0.05, 0.05, 0.15]
obstacle1_size = [0.05, 0.05, 0.05]
obstacle2_size = [0.05, 0.05, 0.10]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = REFERENCE_FRAME
obstacle1_pose.pose = self.pwh.pose[self.o1id]
obstacle1_pose.pose.position.z += 0.025
# Add the target object to the scene
self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
obstacle2_pose = PoseStamped()
obstacle2_pose.header.frame_id = REFERENCE_FRAME
obstacle2_pose.pose = self.pwh.pose[self.o2id]
# Add the target object to the scene
self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
def interactiveMarkerLocationCallback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
ps = PoseStamped()
ps.header.frame_id = feedback.header.frame_id
ps.pose = feedback.pose
self.current_goal_pose = ps
if self.opt.robot == 'pr2':
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
for marker in self.wp_im.controls[0].markers:
marker.color = ColorRGBA(1,1,1,0.4)
self.server.insert(self.wp_im)
self.server.applyChanges()
if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
self._updateIKFeasibility(ps)
self.server.insert(self.wp_im)
self.server.applyChanges()
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
if self.continuous_update:
ps = PoseStamped()
ps.header.frame_id = feedback.header.frame_id
ps.pose = feedback.pose
self.current_goal_pose = ps
#weights_msg = haptic_msgs.HapticMpcWeights()
#weights_msg.header.stamp = rospy.Time.now()
#weights_msg.position_weight = self.pos_weight
#weights_msg.orient_weight = self.orient_weight
#self.mpc_weights_pub.publish(weights_msg) # Enable position and orientation tracking
self.goal_pos_pub.publish(self.current_goal_pose)
self.server.applyChanges()
def distance(start_node, goal_node, make_plan=None):
#Distance between two positions. Both start_node and goal_node are expected to be tuples (x, y)
if make_plan is not None:
start = PoseStamped()
start.header.frame_id = '/map'
start.pose = Pose()
start.pose.position.x = start_node[0]
start.pose.position.y = start_node[1]
goal = PoseStamped()
goal.header.frame_id = '/map'
goal.pose = Pose()
goal.pose.position.x = goal_node[0]
goal.pose.position.y = goal_node[1]
try:
resp = make_plan(start, goal, 0)
if len(resp.plan.poses) > 0:
distance = plan_distance(resp.plan.poses)
else: # The path wasn't computed for whatever reason
distance = euclidean_distance(start_node, goal_node)
except rospy.ServiceException: # The call to make_plan failed
distance = euclidean_distance(start_node, goal_node)
else:
distance = euclidean_distance(start_node, goal_node)
return distance
def do_handshake(self):
rospy.loginfo("Doing handshake")
for i in range(self.num_times_shake):
# go down and wait
ps = PoseStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = self.global_frame_id
ps.pose = deepcopy(self.initial_pose)
ps.pose.position.z += -self.shake_amount_z / 2.0
self.follow_pose_pub.publish(ps)
rospy.sleep(self.handshake_poses_time_step / 2.0)
# go up and wait
rospy.loginfo("Going up for s")
ps.header.stamp = rospy.Time.now()
ps.pose = deepcopy(self.initial_pose)
ps.pose.position.z += self.shake_amount_z / 2.0
self.follow_pose_pub.publish(ps)
rospy.sleep(self.handshake_poses_time_step)
self.deactivate_fingers_interaction()
# go to initial pose and wait
rospy.loginfo("Going back to initial but more back for s")
ps.header.stamp = rospy.Time.now()
ps.pose = deepcopy(self.initial_pose)
ps.pose.position.x += -0.20
self.follow_pose_pub.publish(ps)
rospy.sleep(self.handshake_poses_time_step)
def computeOrientation(start, goal):
global tolerance
rospy.wait_for_service('move_base_node/NavfnROS/make_plan')
try:
# Initiating the connection to the make plan service from the move base node.
make_plan_connection = rospy.ServiceProxy('move_base_node/NavfnROS/make_plan', GetPlan)
start = PoseStamped()
goal = PoseStamped()
start.header.stamp = rospy.get_rostime()
goal.header.stamp = rospy.get_rostime()
start.header.frame_id = '/map'
goal.header.frame_id = '/map'
start.pose = startPose
goal.pose = goalPose
# Calculation of the path by move base
response = make_plan_connection(start,goal,tolerance)
deltaX = start.pose.pose.position.x - path[0].pose.pose.position.x
deltaY = start.pose.pose.position.y - path[0].pose.pose.position.y
yaw = atan2(deltaY,deltaX)
return yaw
except rospy.ServiceException, e:
print "Service call failed: %s" %e
def add_bb_to_planning(self):
coll_obj = rospy.ServiceProxy(self.s_coll_obj, ArmNavCollObj);
mpose = PoseStamped()
mpose.header.frame_id = '/map'
mpose.pose = self.object_pose
print "POSE of object which Im going to add to planning"
print mpose
try:
coll_obj(object_name = self.unknown_object_name,
pose = mpose,
bb_lwh = self.object_bb,
allow_collision=True,
attached=False,
attach_to_frame_id='',
allow_pregrasps=False);
except Exception, e:
rospy.logerr('Cannot add unknown object to the planning scene, error: %s',str(e))
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
rospy.logwarn('One of the frame objects might not exist: ' +
from_frame + ' or ' + to_frame)
return pose
def select_arm_for_pick(self, obj, objects_frame_id, tf_listener):
assert isinstance(obj, ObjInstance)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)
obj_pose = PoseStamped()
obj_pose.pose = obj.pose
obj_pose.header.frame_id = objects_frame_id
# exact time does not matter in this case
obj_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform(
'base_link',
obj_pose.header.frame_id,
obj_pose.header.stamp,
rospy.Duration(1))
obj_pose = tf_listener.transformPose(
'base_link', obj_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def select_arm_for_drill(self, obj_to_drill, objects_frame_id, tf_listener):
assert isinstance(obj_to_drill, ObjInstance)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)
# frameExists("marker") always returns False -> but it is probably not necessary to test it - it should exist
# if tf_listener.frameExists(
# "base_link") and tf_listener.frameExists(ArtBrainUtils.normalize_frame_id(objects_frame_id)):
obj_pose = PoseStamped()
obj_pose.pose = obj_to_drill.pose
obj_pose.header.frame_id = objects_frame_id
# exact time does not matter in this case
obj_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform(
'base_link',
obj_pose.header.frame_id,
obj_pose.header.stamp,
rospy.Duration(1))
obj_pose = tf_listener.transformPose(
'base_link', obj_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def otld_callback(self, data):
print "Heard: " + str(data)
# get the depth image
s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback)
while not self.got_pointcloud:
# do nothing
print "Waiting for pointcloud"
rospy.sleep(0.1)
s.unregister()
self.got_pointcloud = False
# get the depth of the tracked area
# Header header
# int32 x
# int32 y
# int32 width
# int32 height
# float32 confidence
# probably first get the centroid, we should do a median here or something
centerx = data.x + data.width / 2
centery = data.y + data.height / 2
#print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step)
print "centerx: " + str(centerx) + " centery: " + str(centery)
#pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step]
#point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery])
point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]])
print "point is: " + str(point)
for item in point:
print "item is: " + str(item)
print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2])
# create PoseStamped
pose = PoseStamped()
pose.header = std_msgs.msg.Header()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "camera_link"
pose.pose = Pose()
pose.pose.position.x = item[2] # kinect Z value, [2], is X in TF of camera_link
pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link
pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link
pose.pose.orientation.w = 1
# send PoseStamped
self.pub.publish(pose)
arrow = Marker()
arrow.header.frame_id = "camera_link"
arrow.header.stamp = rospy.Time.now()
arrow.type = Marker.ARROW
arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)]
#arrow.points = [Point(0,0,0), Point(0,0,1)]
arrow.scale.x = 0.1 # anchura del palo
arrow.scale.y = 0.15 # anchura de la punta
# arrow.scale.z = 0.1
arrow.color.a = 1.0
arrow.color.r = 1.0
arrow.color.g = 1.0
arrow.color.b = 1.0
self.marker_publisher.publish(arrow)
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def getAnglesFromPose(self,pose):
if type(pose)==Pose:
goal=PoseStamped()
goal.header.frame_id="/base"
goal.pose=pose
else:
goal=pose
if not self.ik:
rospy.logerror("ERROR: Inverse Kinematics service was not loaded")
return None
goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(goal)
try:
resp = self.iksvc(ikreq)
if (resp.isValid[0]):
return resp.joints[0]
else:
rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr)
return None
except rospy.ServiceException,e :
rospy.loginfo("Service call failed: %s" % (e,))
return None
def insert_goal(self, pos_x, pos_y, pos_z):
# 插入一个新点
# target_points在ORB_SLAM/World坐标系下
q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
q = Quaternion(*q_angle)
waypoint = Pose(Point(pos_x, pos_y, pos_z), q)
waypoint_stamped = PoseStamped()
waypoint_stamped.header.frame_id = "map"
waypoint_stamped.header.stamp = rospy.Time(0)
waypoint_stamped.pose = waypoint
self.waypoints.append(waypoint_stamped)
# 转至ORB_SLAM/World坐标系
rospy.loginfo("获取TF map->ORB_SLAM/World")
tf_flag = False
while not tf_flag and not rospy.is_shutdown():
try:
t = rospy.Time(0)
self.listener.waitForTransform("map", "ORB_SLAM/World", t,
rospy.Duration(1.0))
tf_flag = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as e:
tf_flag = False
rospy.logwarn("获取TF失败 map->ORB_SLAM/World")
rospy.logwarn(e)
rospy.loginfo("获取TF 成功 map->ORB_SLAM/World")
target_point = self.listener.transformPose(
"ORB_SLAM/World", waypoint_stamped)
target_point.header.stamp = rospy.Time.now()
self.target_points.append(target_point)
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def get_a_list_of_pose_to_goal(self, start, goal, tf_listener, dist_limit=None, pathpub=None, wppub=None, skip=1):
resolution = self.og.info.resolution
width = self.og.info.width
height = self.og.info.height
offsetX = self.og.info.origin.position.x
offsetY = self.og.info.origin.position.y
tf_listener.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0))
tstart = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(start))
tgoal = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal))
if dist_limit is not None:
tgoal = self.limit_max_dist(tstart, tgoal, dist_limit)
if wppub is not None:
ttgoal = tf_listener.transformPose('map', fuck_the_time(tgoal))
wppub.publish(ttgoal)
start_grid_pos = pose2gridpos(tstart.pose, resolution, offsetX, offsetY)
goal_grid_pos = pose2gridpos(tgoal.pose, resolution, offsetX, offsetY)
try:
path = aStar(self.make_navigable_gridpos(), start_grid_pos, goal_grid_pos, self.astarnodescb)
wp = self.getWaypoints(path, tgoal, publisher=pathpub)
list_of_pose = []
# magic number. skip a few waypoints in the beginning
for stuff in wp:
retp = PoseStamped()
retp.pose = stuff.pose
retp.header = self.og.header
list_of_pose.append(retp)
return list_of_pose
except NoPathFoundException as e:
raise e
def ContourinfoLists_callback(self, contourinfolists):
if self.initialized:
nContours = len(contourinfolists.x)
if (nContours==1):
self.textError = ''
poseContour = PoseStamped(header=contourinfolists.header,
pose=Pose(position=Point(x=contourinfolists.x[0],
y=contourinfolists.y[0]))
)
poseVisual = PoseStamped()
poseVisual.header = contourinfolists.header
poseVisual.header.frame_id = 'Arena'
poseVisual.pose = self.PoseArenaFromImage(poseContour) # Gives the position of the contour in millimeters.
# Point to the cache non-working entry.
iLoading = (self.iWorking+1) % 2
self.cachePose[iLoading] = poseVisual
self.areaVisual = contourinfolists.area[0]
self.eccVisual = contourinfolists.ecc[0]
self.xMin = min(self.xMin, poseVisual.pose.position.x)
self.xMax = max(self.xMax, poseVisual.pose.position.x)
self.yMin = min(self.yMin, poseVisual.pose.position.y)
self.yMax = max(self.yMax, poseVisual.pose.position.y)
elif (nContours==0):
#rospy.logwarn('ERROR: No objects detected.')
self.textError = 'ERROR: No objects detected.'
elif (nContours>1):
#rospy.logwarn('ERROR: More than one object detected.')
self.textError = 'ERROR: More than one object detected.'
def create_pose_stamped(pose, frame_id = 'base_link'):
m = PoseStamped()
m.header.frame_id = frame_id
#m.header.stamp = rospy.get_rostime()
m.header.stamp = rospy.Time()
m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
return m
def getWaypoints(self, list_of_gridpos, goal_pose, publisher=None):
resolution = self.og.info.resolution
width = self.og.info.width
height = self.og.info.height
offsetX = self.og.info.origin.position.x
offsetY = self.og.info.origin.position.y
wp = Path()
wp.header.frame_id = self.og.header.frame_id
poses = []
lastdirection = -999
for i in range(0, len(list_of_gridpos) - 1):
direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1])
if direction != lastdirection:
lastdirection = copy.deepcopy(direction)
newPose = PoseStamped()
newPose.header.frame_id = self.og.header.frame_id
newPose.pose = Pose()
newPose.pose.position = getPoint(list_of_gridpos[i], resolution, offsetX, offsetY)
quaternion = tf.transformations.quaternion_from_euler(0, 0, direction)
newPose.pose.orientation.x = quaternion[0]
newPose.pose.orientation.y = quaternion[1]
newPose.pose.orientation.z = quaternion[2]
newPose.pose.orientation.w = quaternion[3]
poses.append(newPose)
newPose = self.tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal_pose))
poses.append(newPose)
wp.poses = poses
if publisher is not None:
publisher.publish(wp)
return wp.poses
def update_cb(msg):
global planning_started
global tf
global gr_pose
global gr_lock
if len(msg.poses) != 1:
return
if msg.poses[0].name != "MPR 0_end_control":
return
tmp = PoseStamped()
tmp.header.stamp = rospy.Time(0)
tmp.header.frame_id = msg.poses[0].header.frame_id
tmp.pose = msg.poses[0].pose
tf.waitForTransform('/map',msg.poses[0].header.frame_id,rospy.Time(0), rospy.Duration(1.0))
tmp = tf.transformPose('/map',tmp)
gr_lock.acquire()
gr_pose = tmp.pose
gr_lock.release()
if (planning_started == False):
rospy.loginfo('Planning has been started!')
planning_started = True
def grasp_bowl(self, p0, x_angle, frame_id ="/base_link",
side_grasp = False):
dest = np.asarray(p0)
if side_grasp:
rospy.loginfo("Doing a side grasp")
Q = utils.transformations.quaternion_from_euler(
0,
0,
x_angle-np.pi/2)
else:
rospy.loginfo("Doing a top grasp")
Q = utils.transformations.quaternion_from_euler(
x_angle,
np.pi/2,
0)
rospy.loginfo("Got a quaternion of \n%s", Q)
T = utils.transformations.quaternion_matrix(Q)
T[:3, 3] = dest
T[2,3] += 0.16
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = utils.matrixToPose(T)
return ps
def handle_xy_goal(req):
p = PoseStamped()
p.header = req.header
p.pose = req.pose.pose
rospy.loginfo("GPS goal converted to xy: " + str(p.pose.position.x) + ", " + str(p.pose.position.y) + ". Publishing..")
global pub_goal
pub_goal.publish(p)
def sanitizePS(ps):
"""
Converts Pose to PoseStamped()
Converts lists of positions and orientations to the correct datatype
:param ps: pose to repair
:type ps: geometry.msgs.msg.Pose or geometry.msgs.msg.PoseStamped
:return: repaired pose
:rtype: geometry.msgs.msg.PoseStamped
"""
_ps=deepcopy(ps)
if type(_ps)==Pose:
ps=PoseStamped()
ps.header.stamp=rospy.Time().now()
ps.header.frame_id=FRAME_ORIGIN
ps.pose=_ps
elif type(_ps)==PoseStamped:
ps=_ps
else:
raise Exception,"Unhandled data type for sanitizePS: %s"%str(type(_ps))
if type(ps.pose.position)!=Point: # Assume this is a list or numpy array
ps.pose.position=Point(*ps.pose.position)
if type(ps.pose.orientation)!=Quaternion: # Assume this is a list or numpy array
ps.pose.orientation=Quaternion(*ps.pose.orientation)
return ps
def get_hand_frame_pose(self, robot_state=None, frame_id=None):
'''
Returns the pose of the hand in the current or passed in robot state.
Note that this function uses the TransformState get_transform function rather than FK.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find
the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot
state in the planning scene interface.
*frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in,
returns in the robot frame.
**Returns:**
A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
'''
if frame_id == None:
frame_id = self._psi.robot_frame
if robot_state == None:
robot_state = self._psi.get_robot_state()
trans = self._transformer.get_transform(self.hand.hand_frame, frame_id, robot_state)
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = conv.transform_to_pose(trans.transform)
return ps
def positionCallback(event):
# Transforms to the map frame from the base frame
transform.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(2.0))
(pos, orient) = transform.lookupTransform('map', 'base_footprint', rospy.Time(0))
# Updates the x and y values for the robot pose
MyGlobals.robotPose.position.x = pos[0]
MyGlobals.robotPose.position.y = pos[1]
# Updates the orientation for the robot pose
q = [orient[0], orient[1], orient[2], orient[3]]
roll, pitch, yaw = euler_from_quaternion(q)
MyGlobals.robotPose.orientation.x = orient[0]
MyGlobals.robotPose.orientation.y = orient[1]
MyGlobals.robotPose.orientation.z = orient[2]
MyGlobals.robotPose.orientation.w = orient[3]
# Publishes the robots position for visual use in rviz
publishPose = PoseStamped()
publishPose.pose = MyGlobals.robotPose
publishPose.header.frame_id = 'map'
publishPose.header.stamp = rospy.Time(0)
MyGlobals.pubRobot.publish(publishPose)
def map_and_detect( self ):
print "Get object pose"
rospy.sleep(ROBUST_POSE_TIMEOUT)
self.g_object_pose = False
counter=0
while self.g_object_pose == False and counter < DETECTION_TIMEOUT:
self.g_object_pose = self.get_object_pose()
counter = counter+1
if self.g_object_pose == False:
self.state = 12
return False
object_mesh_pose = PoseStamped()
object_mesh_pose.header = Header(stamp = rospy.Time.now(), frame_id = '/world')
object_mesh_pose.pose = self.g_object_pose
object_mesh_pose.pose.orientation = rotate_orientation_quaternion(object_mesh_pose.pose.orientation)
print "Adding the object mesh to the scene"
rospy.wait_for_service('place_object')
req = place_objectRequest();
req.object_pose = object_mesh_pose;
req.object_name = self.g_decision_making_state.object_name;
print self.g_decision_making_state.object_name;
try:
place_object_caller = rospy.ServiceProxy('place_object', place_object)
resp1 = place_object_caller(req);
except rospy.ServiceException, e:
print "Service call failed:"
def go_tool_frame(group, T_tool, base_frame_id='torso_lift_link', ft=False, wait=True, tool_x_offset=0.0):
'''
Plans and executes a moveit motion plan of the PR2 arm's tool frame (l/r_gripper_tool_frame) w.r.t. some base ref frame (default: 'torso_lift_link')
:param group: MoveGroupCommander type (left_arm or right_arm).
:param T_tool: kdl.Frame or geometry_msgs.Pose. Desired pose of the tool frame w.r.t some base frame.
:param base_frame_id: string, frame ID of the base in which T_tool is expressed. Default: 'torso_lift_link'
:param ft: bool. True if the arm has a force-torque sensor, false otherwise
:param wait: True/False to wait for execution of motion plan
:param tool_x_offset: (double) offset in tool length
:return:
'''
T_wrist = transform_wrist_frame(T_tool, ft, tool_x_offset)
T_wrist_stamped = PoseStamped()
T_wrist_stamped.pose = T_wrist
T_wrist_stamped.header.frame_id = base_frame_id
T_wrist_stamped.header.stamp = rospy.Time.now()
result = group.go(T_wrist_stamped, wait)
if not result:
raise Exception('NO MOTION PLAN FOUND')
return result
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
fail_ct = 0
break
fail_ct += 1
# Tuck the arm
#grasping_client.tuck()
# Place the block
while not rospy.is_shutdown() and cube_in_grapper:
rospy.loginfo("Placing object...")
pose = PoseStamped()
#pose.pose = cube.primitive_poses[0]
#pose.pose.position.x += 0.10
##pose.pose.position.y = -1.0
#pose.pose.position.z += 0.02
pose.pose = obj_goal_1
#pose.pose.position.x = 0.6
#pose.pose.position.y = 0.182#-0.793
#pose.pose.position.z = 0.65
pose.header.frame_id = cube.header.frame_id
if grasping_client.place(cube, pose):
cube_in_grapper = False
break
rospy.logwarn("Placing failed.")
grasping_client.intermediate_stow()
grasping_client.stow()
if fail_ct > 15:
fail_ct = 0
break
fail_ct += 1
# Tuck the arm, lower the torso
def list_to_pose_stamped(pose_list, target_frame):
pose_msg = PoseStamped()
pose_msg.pose = list_to_pose(pose_list)
pose_msg.header.frame_id = target_frame
pose_msg.header.stamp = rospy.Time.now()
return pose_msg
def bag_tf_poses(timed_pos_quat,
bagpath,
child_frame_id="vins",
red=1.0,
green=1.0,
blue=0.0):
"""
:param timed_pos_quat:
:param bagpath:
:return:
"""
if os.path.isfile(bagpath):
bag = rosbag.Bag(bagpath, "a")
else:
bag = rosbag.Bag(bagpath, "w")
path = Path()
for index, entry in enumerate(timed_pos_quat):
odometry = Odometry()
odometry.header.stamp = entry[0]
odometry.header.seq = index
odometry.header.frame_id = "world"
odometry.child_frame_id = child_frame_id
odometry.pose.pose.position.x = entry[1]
odometry.pose.pose.position.y = entry[2]
odometry.pose.pose.position.z = entry[3]
odometry.pose.pose.orientation.x = entry[4]
odometry.pose.pose.orientation.y = entry[5]
odometry.pose.pose.orientation.z = entry[6]
odometry.pose.pose.orientation.w = entry[7]
odometry.twist.twist.linear.x = 0
odometry.twist.twist.linear.y = 0
odometry.twist.twist.linear.z = 0
pose_stamped = PoseStamped()
pose_stamped.header.stamp = entry[0]
pose_stamped.header.seq = index
pose_stamped.header.frame_id = "world"
pose_stamped.pose = odometry.pose.pose
path.header.stamp = entry[0]
path.header.seq = index
path.header.frame_id = "world"
path.poses.append(pose_stamped)
if len(path.poses) > 2:
path.poses.pop(0)
marker = rviz_camera_frustum.generate_frustum_marker(
entry[1:], 1.0, entry[0], red, green, blue)
marker.header.frame_id = "world"
marker.header.stamp = entry[0]
marker.header.seq = index
# see https://wiki.ros.org/rviz/DisplayTypes/Marker#Triangle_List_.28TRIANGLE_LIST.3D11.29_.5B1.1.2B-.5D for drawing complex markers
bag.write("/path_{}".format(child_frame_id), path, entry[0])
bag.write("/odom_{}".format(child_frame_id), marker, entry[0])
bag.close()
def get_fk_pose(self, root, tip):
homo_m = self.get_fk_np(root, tip)
p = PoseStamped()
p.header.frame_id = root
p.pose = homo_matrix_to_pose(homo_m)
return p
Quaternion(0.173, -0.693, -0.242, 0.657)),
Pose(Point(0.047, 0.545, 1.822),
Quaternion(-0.274, -0.701, 0.173, 0.635))
]
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
while not rospy.is_shutdown():
for pose in gripper_poses:
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
# Move gripper frame to the pose specified
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Hello there!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
def run(args):
rospy.init_node('chaser_control')
# Update paths every 100 ms.
rate_limiter = rospy.Rate(10)
rospy.Subscriber('/captured_runners', String, capture_runner)
publishers = {
'c' + str(i): rospy.Publisher('/c' + str(i) + '/path',
PoseArray,
queue_size=1)
for i in range(3)
}
if RVIZ_PUBLISH:
runner_est_publisher = rospy.Publisher('/runner_particles',
PointCloud,
queue_size=1)
path_publishers = {}
for c in CHASERS:
path_publishers[c] = rospy.Publisher('/' + c + '_path',
Path,
queue_size=1)
map_publisher = rospy.Publisher('/map',
ros_nav.OccupancyGrid,
queue_size=1)
map_publisher.publish(OCC_GRID.get_ros_message())
gts = MultiGroundtruthPose(CHASERS + RUNNERS)
runner_ests = {'r0': None, 'r1': None, 'r2': None}
last_seen = {'r0': None, 'r1': None, 'r2': None}
last_target = ''
frame_id = 0
print('Chaser controller initialized.')
while not rospy.is_shutdown():
# Make sure all groundtruths are ready
if not gts.ready:
rate_limiter.sleep()
continue
chaser_positions = []
for c in CHASERS:
chaser_positions.append(gts.poses[c][:2])
# Update estimated runner positions
for r in RUNNERS:
r_pos = gts.poses[r][:2]
visible = False
for c_pos in chaser_positions:
if in_line_of_sight(r_pos, c_pos):
if not runner_ests[r] is None:
print('Runner ' + r + ' found at ' + str(r_pos) +
' by chaser at ' + str(c_pos) + '.')
runner_ests[r] = None
last_seen[r] = r_pos
visible = True
break
if not visible:
if runner_ests[r] is None:
if last_seen[r] is None:
runner_ests[r] = ParticleCloud(NUM_CLOUD_POINTS,
CHASER_SPEED,
chaser_positions, r)
else:
runner_ests[r] = ParticleCloud(NUM_CLOUD_POINTS,
CHASER_SPEED,
chaser_positions, r,
last_seen[r])
else:
runner_ests[r].update(chaser_positions)
# Allocate chasers to runners
least_dist = np.inf
for r in RUNNERS:
for c in CHASERS:
if not runner_ests[r] is None:
# Max distance from a point in the point cloud
dist = np.max([
np.linalg.norm(gts.poses[c][:2] - r_pos)
for r_pos in runner_ests[r].get_positions()
])
else:
dist = np.linalg.norm(gts.poses[c][:2] - gts.poses[r][:2])
if dist < least_dist:
least_dist = dist
target_runner = r
if last_target != target_runner:
print('New target runner allocated: ' + target_runner)
last_target = target_runner
allocations = {}
for c in CHASERS:
allocations[c] = target_runner
# Calculate chaser paths
path_tail_max = 15
occupancy_grid = get_occupancy_grid()
potential_field = PotentialField({}, is_path=True)
paths = {}
chasers = ['c0', 'c1', 'c2']
if not runner_ests[target_runner] is None:
chasers_ordered = sorted(
chasers,
key=lambda x: np.max([
np.linalg.norm(gts.poses[x][:2] - r_pos)
for r_pos in runner_ests[target_runner].get_positions()
]))
else:
chasers_ordered = sorted(
chasers,
key=lambda x: np.linalg.norm(gts.poses[x][:2] - gts.poses[
allocations[x]][:2]))
for c in chasers_ordered:
#plots.plot_field(potential_field, ARENA_OFFSET)
c_id = int(c[1])
start_pose = gts.poses[c]
# Estimate or get goal position
target_runner = allocations[c]
if runner_ests[target_runner] is None:
goal_position = gts.poses[target_runner][:2]
else:
# Sample closest position in point cloud
min_dist = np.inf
for r_pos in runner_ests[target_runner].get_positions():
dist = np.linalg.norm(start_pose[:2] - r_pos[:2])
if dist < min_dist:
goal_position = r_pos
min_dist = dist
# Get potential field
targets = []
for other_c_id in range(3):
if other_c_id == c_id:
continue
if allocations[chasers[other_c_id]] == target_runner:
targets.append(chasers[other_c_id])
#plots.plot_field(potential_field, 8)
path, s, g = rrt_star_path(start_pose,
goal_position,
occupancy_grid,
potential_field,
targets=targets)
path_tail = path[-min(len(path), path_tail_max):]
potential_field.add_target(c, [path_tail, 1.5, 10.])
path_arr = [position_to_point(point) for point in path]
paths[c] = path_arr
# Publish chaser paths
path_msg = create_pose_array(path_arr)
publishers[c].publish(path_msg)
# Update particle clouds
chaser_positions = []
for c_name in CHASERS:
chaser_positions.append(gts.poses[c_name][:2])
for r in RUNNERS:
r_pos = gts.poses[r][:2]
visible = False
for c_pos in chaser_positions:
if in_line_of_sight(r_pos, c_pos):
if not runner_ests[r] is None:
print('Runner ' + r + ' found at ' + str(r_pos) +
' by chaser at ' + str(c_pos) + '.')
runner_ests[r] = None
last_seen[r] = r_pos
visible = True
break
if not visible:
if runner_ests[r] is None:
if last_seen[r] is None:
runner_ests[r] = ParticleCloud(
NUM_CLOUD_POINTS, CHASER_SPEED,
chaser_positions, r)
else:
runner_ests[r] = ParticleCloud(
NUM_CLOUD_POINTS, CHASER_SPEED,
chaser_positions, r, last_seen[r])
else:
runner_ests[r].update(chaser_positions)
# Publish localization particles and chaser positions
if RVIZ_PUBLISH:
particle_msg = PointCloud()
particle_msg.header.seq = frame_id
particle_msg.header.stamp = rospy.Time.now()
particle_msg.header.frame_id = '/odom'
intensity_channel = ChannelFloat32()
intensity_channel.name = 'intensity'
particle_msg.channels.append(intensity_channel)
for r in RUNNERS:
r_est = runner_ests[r]
if not r_est is None:
for p in r_est.get_positions():
pt = Point()
pt.x = p[X]
pt.y = p[Y]
pt.z = .05
particle_msg.points.append(pt)
intensity_channel.values.append(int(r[1]))
runner_est_publisher.publish(particle_msg)
path_msg = Path()
path_msg.header.seq = frame_id
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = '/odom'
for position in paths[c]:
ps = PoseStamped()
ps.header.seq = frame_id
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = '/odom'
p = Pose()
p.position = position
ps.pose = p
path_msg.poses.append(ps)
path_publishers[c].publish(path_msg)
rate_limiter.sleep()
frame_id += 1
np_speeds = lr.speeds(tees)
return(np_speeds)
model = keras.models.load_model('data/modelRnnFortieth15a')
print(model.summary())
# main program constants
# testing
target_odometry = Odometry()
current_pose = PoseStamped()
current_pose.header.frame_id = 't265_pose_frame'
current_pose.pose.orientation.w = 1.0
target_pose = PoseStamped()
target_pose.pose = target_odometry.pose.pose
target_twist = TwistStamped()
target_twist.twist = target_odometry.twist.twist
target_pose.header.frame_id = 't265_pose_frame'
target_twist.header.frame_id = 't265_pose_frame'
target_twist.header.frame_id = 't265_pose_frame'
target_pose.pose.orientation.w = 1.0
# set some sample values
target_pose.pose.position.x = 0.0
target_pose.pose.position.y = 0.0
qa_orig = q_to_array(current_pose.pose.orientation)
qa_rot = tf.transformations.quaternion_from_euler(0, 0, 0 * D_TO_R)
qa_new = tf.transformations.quaternion_multiply(qa_rot, qa_orig)
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]
# A list of pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for pitch in pitch_vals:
for dy in y_vals:
for dx in x_vals:
place.pose.position.x = init_pose.pose.position.x + dx
place.pose.position.y = init_pose.pose.position.y + dy
# Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
# arm base to the place location (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw = atan2(y, x)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
# the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
# objects orientation!), so we cancel this transformation. It is applied here:
# https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
# More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
acobjs = self.scene.get_attached_objects([target_id])
aco_pose = self.get_pose(acobjs[target_id])
if aco_pose is None:
rospy.logerr("Attached collision object '%s' not found" % target_id)
return None
aco_tf = self.pose_to_mat(aco_pose)
place_tf = self.pose_to_mat(place.pose)
place.pose = self.mat_to_pose(place_tf, aco_tf)
rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
% (aco_pose, place.pose))
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def test_get_ik(self):
pose = PoseStamped()
pose.header.stamp = rospy.get_rostime()
pose.pose = conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1])
joint_state_from_ik = self.robot_commander.get_ik(pose)
self.assertIsInstance(joint_state_from_ik, JointState)
0.0,
])
# Move to second table
rospy.loginfo("Moving to second table...")
move_base.goto(-3.53, 3.75, 1.57)
move_base.goto(-3.53, 4.15, 1.57)
# Raise the torso using just a controller
rospy.loginfo("Raising torso...")
torso_action.move_to([
0.4,
])
# Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if grasping_client.place(cube, pose):
break
rospy.logwarn("Placing failed.")
# Tuck the arm, lower the torso
grasping_client.tuck()
torso_action.move_to([
0.0,
])
def transform_stamped_msg_to_pose_stamped_msg(transform_stamped_msg):
assert isinstance(pose, TransformStamped), 'Input is not of type geometry_msgs/TransformStamped'
pose_stamped_msg = PoseStamped()
pose_stamped_msg.header = transform_stamped_msg.header
pose_stamped_msg.pose = transform_stamped_msg_to_pose_msg(transform_stamped_msg)
return pose_stamped_msg
def matrix_to_pose_stamped_msg(matrix, frame_id):
pose_stamped_msg = PoseStamped()
pose_stamped_msg.header.frame_id = frame_id
pose_stamped_msg.header.stamp = rospy.Time.now()
pose_stamped_msg.pose = matrix_to_pose_msg(matrix)
return pose_stamped_msg
def run(self):
ltime = time.time()
idx = 0
while True:
if (time.time() - ltime >= 1 / self.frequency):
if (idx == len(self.left_img_list)):
idx = 0
# RGB Images
left_img = self.read_image_file(self.left_img_list[idx])
right_img = self.read_image_file(self.right_img_list[idx])
header = Header()
set_timestamp(header, time.time())
header.frame_id = "left_img"
left_msg = from_ndarray(left_img, header)
self.publish("left_img", left_msg)
header.frame_id = "right_img"
right_msg = from_ndarray(right_img, header)
self.publish("right_img", right_msg)
# Depth Images
left_depth = self.read_numpy_file(self.left_depth_list[idx])
left_depth_vis = depth2vis(left_depth)
header.frame_id = "left_depth"
left_msg = from_ndarray(left_depth_vis, header)
self.publish("left_depth", left_msg)
right_depth = self.read_numpy_file(self.right_depth_list[idx])
right_depth_vis = depth2vis(right_depth)
header.frame_id = "right_depth"
right_msg = from_ndarray(right_depth_vis, header)
self.publish("right_depth", right_msg)
# Semantic Segmentation
left_seg = self.read_numpy_file(self.left_seg_list[idx])
left_seg_vis = seg2vis(left_seg)
header.frame_id = "left_segmentation"
left_msg = from_ndarray(left_seg_vis, header)
self.publish("left_segmentation", left_msg)
right_seg = self.read_numpy_file(self.right_seg_list[idx])
right_seg_vis = seg2vis(right_seg)
header.frame_id = "right_segmentation"
right_msg = from_ndarray(right_seg_vis, header)
self.publish("right_segmentation", right_msg)
# Left Camera Pose
pose_stamped = PoseStamped()
pose_stamped.header = header
pose_stamped.header.frame_id = "left_camera"
pose = Pose()
pose.position.x = self.pose_l[idx][0]
pose.position.y = self.pose_l[idx][1]
pose.position.z = self.pose_l[idx][2]
pose.orientation.x = self.pose_l[idx][3]
pose.orientation.y = self.pose_l[idx][4]
pose.orientation.z = self.pose_l[idx][5]
pose.orientation.w = self.pose_l[idx][6]
pose_stamped.pose = pose
self.publish("left_pose", pose_stamped)
# Right Camera Pose
pose_stamped = PoseStamped()
pose_stamped.header = header
pose_stamped.header.frame_id = "right_camera"
pose = Pose()
pose.position.x = self.pose_r[idx][0]
pose.position.y = self.pose_r[idx][1]
pose.position.z = self.pose_r[idx][2]
pose.orientation.x = self.pose_r[idx][3]
pose.orientation.y = self.pose_r[idx][4]
pose.orientation.z = self.pose_r[idx][5]
pose.orientation.w = self.pose_r[idx][6]
pose_stamped.pose = pose
self.publish("right_pose", pose_stamped)
if (idx > 0):
flow = self.read_numpy_file(self.flow_list[idx - 1])
flow_vis = flow2vis(flow)
header.frame_id = "optical_flow"
left_msg = from_ndarray(flow_vis, header)
self.publish("optical_flow", left_msg)
flow_mask = self.read_numpy_file(self.flow_mask_list[idx -
1])
flow_vis_w_mask = flow2vis(flow, mask=flow_mask)
header.frame_id = "optical_flow_mask"
right_msg = from_ndarray(flow_vis_w_mask, header)
self.publish("optical_flow_mask", right_msg)
ltime = time.time()
idx += 1
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = "map"
msg.pose = pose
return msg
def picknplace():
# define the initial positions of baxter's joints in a dictionary
initial_position = {'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569,
'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143,
'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506,
'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871,
'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719}
# create two lists, one of initial locations and one of goal locations
# for BAXTER to move them from one to another
locs_x = [] # initial locations
locs_y = []
orien = []
goal_move = []
place_goal = [] # goal locations
# using readlines() to read the plan.ipc file, the PDDL problem solution
PDDL_solved = open('plan.ipc', 'r')
Lines = PDDL_solved.readlines()
# adding the goal positions from the PDDL solved file
for line in Lines:
orien.append(0);
goal_move.append(line.split()[3][8:9])
cord_dest = int(line.split()[2][8:9])
# adding an offset to all location but the 5th
if(cord_dest != 5):
locs_x.append(BLOCKS[cord_dest - 1][0] + 0.025)
locs_y.append(BLOCKS[cord_dest - 1][1] + 0.03)
else:
locs_x.append(BLOCKS[cord_dest - 1][0])
locs_y.append(BLOCKS[cord_dest - 1][1])
for i in goal_move:
place_goal_i = geometry_msgs.msg.Pose()
if(int(i) != 5):
place_goal_i.position.x = BLOCKS[int(i) - 1][0] + 0.025
place_goal_i.position.y = BLOCKS[int(i) - 1][1] + 0.03
else:
place_goal_i.position.x = BLOCKS[int(i) - 1][0]
place_goal_i.position.y = BLOCKS[int(i) - 1][1]
place_goal_i.position.z = BLOCKS[int(i) - 1][2]
place_goal_i.orientation.x = 1.0
place_goal_i.orientation.y = 0.0
place_goal_i.orientation.z = 0.0
place_goal_i.orientation.w = 0.0
place_goal.append(place_goal_i)
# the distance from the zero point in Moveit to the ground is 0.903 m (for the table we use)
# the value is not always the same. (look in Readme)
center_z_cube = -MOVEIT_Z_CONST + TABLE_SIZE[2] + BLOCK_SIZE[2] / 2
# initialize a list for the objects and the stacked cubes.
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
both_arms.set_joint_value_target(initial_position)
both_arms.plan()
both_arms.go(wait=True)
# remove models from the scene on shutdown
#rospy.on_shutdown(delete_gazebo_models)
# wait for All Clear message from emulator startup.
rospy.wait_for_message("/robot/sim/started", Empty)
planningScene.clear()
"""
Attach a box into the planning scene.
def moveit_python.planning_scene_interface.PlanningSceneInterface.attachBox (self,
name, - Name of the object
size_x, - The x-dimensions size of the box
size_y, - The y-dimensions size of the box
size_z, - The z-dimensions size of the box
x, - The x position in link_name frame
y, - The y position in link_name frame
z, - The z position in link_name frame
link_name, - Name of link to attach this object to
touch_links = None, - Names of robot links that can touch this object
detach_posture = None,
weight = 0.0,
wait = True - When true, we wait for planning scene to actually
update, this provides immunity against lost messages.
)
add the table as attached object
"""
planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2],
'base', touch_links=['pedestal'])
planningScene.waitForSync()
rightgripper.open()
# cProfile explanation - https://docs.python.org/2/library/profile.html#module-cProfile
# cProfile to measure the performance (time) of the task.
profile = cProfile.Profile()
profile.enable()
# loop performing "pick and place" till all objects are cleared from table.
# locs_x,locs_y are the source fot he cubes.
num_of_pick_n_place = 0
while num_of_pick_n_place < len(goal_move):
# do the task only if there are still objects on the table
moved_objects = 0
while moved_objects < len(locs_x):
# clear planning scene
planningScene.clear()
# Add table as attached object.
planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1],
CENTER[2], 'base', touch_links=['pedestal'])
# Initialize the data of the objects on the table.
x_n = locs_x[moved_objects]
y_n = locs_y[moved_objects]
z_n = Z_N_INIT_VALUE
th_n = orien[moved_objects]
# if the angle is bigger than 45 degrees, I assume there will be problem with the pick,
# therefore in that case we need to calibrate the angle to be in the range[-45,45] degrees.
# I surmise the offset of the angle is always adding toward the positive axis for theta,
# thus we need to calibrate theta toward the negative 45.
if th_n > pi / 4:
th_n = -1 * (th_n % (pi / 4))
# Add the detected objects into the planning scene.
for i in range(1, len(locs_x)):
planningScene.addBox(objlist[i], BLOCK_SIZE[0], BLOCK_SIZE[1], BLOCK_SIZE[2], locs_x[i], locs_y[i],
center_z_cube)
planningScene.waitForSync()
# defien a middle point from initial point to goal, as "approach_pick_goal".
# initalize it as equal to the actual pick_goal in z and y axis, and different in z axis
approach_pick_goal = geometry_msgs.msg.Pose()
approach_pick_goal.position.x = x_n
approach_pick_goal.position.y = y_n
approach_pick_goal.position.z = z_n + MIDDLE_POINT_Z
# PoseStamped is a Pose with reference coordinate frame and timestamp
approach_pick_goal_dummy = PoseStamped()
""""
Header is Standard metadata for higher-level stamped data types.
This is generally used to communicate timestamped data in a particular coordinate frame.
sequence ID: consecutively increasing ID uint32 seq
Two-integer timestamp that is expressed as:
* stamp.sec: seconds (stamp_secs) since epoch (called 'secs')
* stamp.nsec: nanoseconds since stamp_secs (called 'nsecs')
"""
approach_pick_goal_dummy.header.frame_id = "base"
approach_pick_goal_dummy.header.stamp = rospy.Time.now()
approach_pick_goal_dummy.pose.position.x = x_n
approach_pick_goal_dummy.pose.position.y = y_n
approach_pick_goal_dummy.pose.position.z = z_n + MIDDLE_POINT_Z
approach_pick_goal_dummy.pose.orientation.x = 1.0
approach_pick_goal_dummy.pose.orientation.y = 0.0
approach_pick_goal_dummy.pose.orientation.z = 0.0
approach_pick_goal_dummy.pose.orientation.w = 0.0
# orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
approach_pick_goal_dummy.pose = rotate_pose_msg_by_euler_angles(approach_pick_goal_dummy.pose, 0.0, 0.0,
th_n)
approach_pick_goal.orientation.x = approach_pick_goal_dummy.pose.orientation.x
approach_pick_goal.orientation.y = approach_pick_goal_dummy.pose.orientation.y
approach_pick_goal.orientation.z = approach_pick_goal_dummy.pose.orientation.z
approach_pick_goal.orientation.w = approach_pick_goal_dummy.pose.orientation.w
# Move to the approach goal and the pick_goal.
pick_goal = deepcopy(approach_pick_goal) # create an exact copy of approach_pick_goal
pick_goal.position.z = z_n
move('right', approach_pick_goal) # move is a function
time.sleep(5)
move('right', pick_goal)
time.sleep(1)
rightgripper.close() # PICK!
time.sleep(1)
# move back to the approach_pick_goal.
move('right', approach_pick_goal)
# define the approach_place_goal
approached_place_goal = deepcopy(place_goal[num_of_pick_n_place])
approached_place_goal.position.z = approached_place_goal.position.z + MIDDLE_POINT_Z
move('right', approached_place_goal)
time.sleep(5)
place_goal_val = place_goal[num_of_pick_n_place]
place_goal_val.position.z = place_goal_val.position.z + BLOCK_SIZE[2]
move('right', place_goal_val)
time.sleep(1)
rightgripper.open() # PLACE!
time.sleep(1)
# move to the approach_place_goal.
move('right', approached_place_goal)
# increase iterators
num_of_pick_n_place += 1
moved_objects += 1
# move the arms to initial position.
both_arms.set_joint_value_target(initial_position)
both_arms.plan()
both_arms.go(wait=True)
profile.disable()
# pstats documentation - https://docs.python.org/3/library/profile.html#module-pstats
pstats.Stats(profile).sort_stats('cumulative').print_stats(0.0)
# exit MoveIt and shutting down the process
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
time.sleep(10)
def isGraspable(collisionObject, arm):
objectType = SolidPrimitive()
GRIP_SIZE = 0.1
graspList = []
# The possible grasp depends on the type of the object (BOX or CYLINDER)
if collisionObject.primitives[0].type == objectType.CYLINDER:
# Poses depends on the arm group used
if arm == gr:
testPoses = rightPoses
elif arm == gl:
testPoses = leftPoses
# If the diameter of the cylinder is smaller than the size of the grippers
if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE:
for quat in testPoses:
grasp = PoseStamped()
grasp.header = deepcopy(collisionObject.header)
grasp.pose = deepcopy(collisionObject.primitive_poses[0])
grasp.pose.orientation = quat
# Fine tuning of the grasp position
if quat == right_side:
grasp.pose.position.x = 0.97 * grasp.pose.position.x
grasp.pose.position.y = grasp.pose.position.y + min(
(collisionObject.primitives[0].dimensions[1]), 0.05
) #account for object being too tall most of the time
grasp.pose.position.z = grasp.pose.position.z - 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
elif quat == left_side:
grasp.pose.position.z = grasp.pose.position.z - 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
grasp.pose.position.y = grasp.pose.position.y - min(
(collisionObject.primitives[0].dimensions[1]), 0.05
) #account for object being too tall most of the time
elif quat == front:
grasp.pose.position.y = 0.975 * grasp.pose.position.y
elif quat == frontOriented:
grasp.pose.position.y = 0.975 * grasp.pose.position.y
grasp.pose.position.z = grasp.pose.position.z - 0.5 * (
collisionObject.primitives[0].dimensions[0]
) #account for object being too tall most of the time
elif quat == top:
grasp.pose.position.z = grasp.pose.position.z + collisionObject.primitives[
0].dimensions[0] / 2 - 0.08
graspList.append(grasp)
# Box grasp is not fined tuned yet !
elif collisionObject.primitives[0].type == objectType.BOX:
grasp = PoseStamped()
grasp.header = deepcopy(collisionObject.header)
grasp.pose = deepcopy(collisionObject.primitive_poses[0])
grasp2 = deepcopy(grasp)
grasp3 = deepcopy(grasp)
# Orientation of the object in the base frame
quat = [
grasp.pose.orientation.x, grasp.pose.orientation.y,
grasp.pose.orientation.z, grasp.pose.orientation.w
]
euler = euler_from_quaternion(quat)
yaw = -(euler[2] + pi / 2)
# If the object is small enough to be grasped
if collisionObject.primitives[0].dimensions[0] <= GRIP_SIZE:
# top
quat_top = quaternion_from_euler(pi, 0.0, yaw, axes='rxyz')
top_box.x = quat_top[0]
top_box.y = quat_top[1]
top_box.z = quat_top[2]
top_box.w = quat_top[3]
grasp.pose.position.z = grasp.pose.position.z + 0.5 * collisionObject.primitives[
0].dimensions[
2] - 0.08 #account for object being too tall most of the time
grasp.pose.orientation = top_box
graspList.append(grasp)
# side
quat_side = quaternion_from_euler(pi, yaw, pi / 2, axes='rxzy') #
box_side.x = quat_side[0]
box_side.y = quat_side[1]
box_side.z = quat_side[2]
box_side.w = quat_side[3]
grasp2.pose.orientation = box_side
graspList.append(grasp2)
if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE:
# front
quat_front = quaternion_from_euler(pi,
yaw - pi / 2,
pi / 2,
axes='rxzy')
box_front.x = quat_front[0]
box_front.y = quat_front[1]
box_front.z = quat_front[2]
box_front.w = quat_front[3]
grasp3.pose.orientation = box_front
graspList.append(grasp3)
return graspList
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
def update_curve(self, data):
# data should contain Path, with multiple, that represent a discretized curve
# we will only use the x,y component for now
points = []
for p in data.poses:
points.append(p.pose.position)
if len(points) < 1:
print('No curve received')
return
line = None
# the segment intersects a circle of radius r if
# the first point is closer than r and the second is further
# we also want the 'last' one that intersects, not the first
# that particular segment is 'forward'.
# p1 inside, p2 outside should not happen for more than 1 point
selfpos = self.pos[:2]
for i in range(len(points) - 1, 0, -1):
p1 = (points[i - 1].x, points[i - 1].y, points[i - 1].z)
p2 = (points[i].x, points[i].y, points[i].z)
p1d = G.euclid_distance(selfpos, p1[:2])
p2d = G.euclid_distance(selfpos, p2[:2])
#print(p1d, p2d)
if p1d > config.LOOK_AHEAD_R:
print('p1d:', p1d, 'p2d:', p2d)
# the first point is inside, check the second one
if p2d < config.LOOK_AHEAD_R:
# we are intersecting!
line = (p1, p2)
if line is None:
print('No segment in range, using first segment')
p1 = (points[0].x, points[0].y, points[0].z)
p2 = (points[1].x, points[1].y, points[1].z)
line = (p1, p2)
p1, p2 = line
p1 = list(p1)
#p1[0] += 3
#p1[1] += 3
line = [p1, p2]
# set these to be used later
self._current_line = line
# self._frame_id = data.header.frame_id
# elongate the line for visualization purposes
x1, y1, z1 = line[0]
x2, y2, z2 = line[1]
slope = (y2 - y1) / (x2 - x1)
d = -5
x2 += d
y2 += d * slope
x1 -= d
y1 -= d * slope
# publish the current line
pose1 = Pose()
pose1.position.x = x1
pose1.position.y = y1
pose1.position.z = z1
stamped1 = PoseStamped()
stamped1.pose = pose1
pose2 = Pose()
pose2.position.x = x2
pose2.position.y = y2
pose2.position.z = z2
stamped2 = PoseStamped()
stamped2.pose = pose2
path = Path()
path.poses = [stamped1, stamped2]
path.header.frame_id = self._frame_id
self.debug_line_pub.publish(path)
def move_line(self, event):
# 直角坐标系运动
event_object = event.GetEventObject()
label = event_object.GetLabel()
if label[2] == '-':
if self.distance >= 0.0:
self.distance = 0.0 - self.distance
elif label[2] == '+':
if self.distance <= 0.0:
self.distance = 0.0 - self.distance
self.text1.SetLabel(label + ' ' + str(abs(self.distance)))
self.traj_options.interpolation_type = 'CARTESIAN'
self.traj.set_trajectory_options(trajectory_options=self.traj_options)
endpoint_state = self.limb.tip_state(self.tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s',
self.tip_name)
return None
pose = endpoint_state.pose
cartesion_axis = label[0].lower()
if cartesion_axis == 'x':
pose.position.x += self.distance
elif cartesion_axis == 'y':
pose.position.y += self.distance
elif cartesion_axis == 'z':
pose.position.z += self.distance
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = self.limb.joint_ordered_angles()
self.waypoint.set_cartesian_pose(poseStamped, self.tip_name,
joint_angles)
self.traj.clear_waypoints() #清除保留的点
self.traj.append_waypoint(self.waypoint.to_msg())
result = self.traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
self.limb.exit_control_mode()
# self.traj.clear_waypoints()
result_pose = self.limb.tip_state(self.tip_name).pose
display_pose = u'位置:\n'
display_pose += '\t' + 'X: ' + str(
"%.4f" % result_pose.position.x) + '\n'
display_pose += '\t' + 'y: ' + str(
"%.4f" % result_pose.position.y) + '\n'
display_pose += '\t' + 'Z: ' + str(
"%.4f" % result_pose.position.z) + '\n'
display_pose += u'姿态:\n'
display_pose += '\t' + 'X: ' + str(
"%.4f" % result_pose.orientation.x) + '\n'
display_pose += '\t' + 'Y: ' + str(
"%.4f" % result_pose.orientation.y) + '\n'
display_pose += '\t' + 'Z: ' + str(
"%.4f" % result_pose.orientation.z) + '\n'
display_pose += '\t' + 'W: ' + str(
"%.4f" % result_pose.orientation.w) + '\n'
self.text_pose.SetLabel(display_pose)
# 显示关节角度
result_joints_angles = self.limb.joint_angles()
display_angles = ''
for key, value in result_joints_angles.items():
display_angles = key[6:8] + ': ' + str(
"%.6f" % value) + '\n' + display_angles
display_angles = u'关节角度：\n' + display_angles
self.text_joint_angles.SetLabel(display_angles)
def callback(data):
currentFrame = data.frameID
for pose in data.poses:
pedID = pose.pedID
if pedsQueue.has_key(pedID):
queue = pedsQueue[pedID]
if len(queue) == QUEUE_SIZE:
queue.pop(0)
queue.append((pose.frameID, pose.x, pose.y))
# queue.sort()
else:
queue = [(pose.frameID, pose.x, pose.y)]
pedsQueue[pedID] = queue
for pedID in pedsQueue:
queue = pedsQueue[pedID]
observation_length = len(queue)
if currentFrame - queue[-1][0] >= 30:
del pedsQueue[pedID]
continue
if observation_length < 3:
continue
# frames = [data for data in range(observation_length+prediction_length,0,-1)]
# xs = [data[1] for data in queue]
# ys = [data[2] for data in queue]
# x_pred = np.zeros(prediction_length)
# y_pred = np.zeros(prediction_length)
# p_x = np.polyfit(frames[:], xs[:], polyfit_degree)
# p_y = np.polyfit(frames[:], ys[:], polyfit_degree)
# p_x = np.poly1d(p_x)
# p_y = np.poly1d(p_y)
# startingFrame = frames[0]
# for i in range(prediction_length):
# x_pred[i] = p_x(startingFrame+i+1)
# y_pred[i] = p_y(startingFrame+i+1)
# validate model
# total_length = len(queue)
# if total_length < 12:
# continue
# observation_length = total_length-prediction_length
frames = [frame for frame in range(observation_length)]
xs = [data[1] for data in queue]
ys = [data[2] for data in queue]
x_pred = np.zeros(prediction_length)
y_pred = np.zeros(prediction_length)
p_x = np.polyfit(frames[:observation_length], xs[:observation_length],
polyfit_degree)
p_y = np.polyfit(frames[:observation_length], ys[:observation_length],
polyfit_degree)
p_x = np.poly1d(p_x)
p_y = np.poly1d(p_y)
# shape_error = (prediction_length, 2)
# error = np.zeros(shape_error)
for i in range(prediction_length):
if isTest:
x_pred[i] = 8
y_pred[i] = 3.8 - i * 0.2
else:
x_pred[i] = p_x(observation_length + i)
y_pred[i] = p_y(observation_length + i)
# error[i][0] = x_pred[i] - xs[i + observation_length]
# error[i][1] = y_pred[i] - ys[i + observation_length]
pointArray = PointArray()
pointArray.header.stamp = rospy.Time.now()
pointArray.header.frame_id = "velodyne"
pose_list_pred = list()
my_path_pred = Path()
my_path_pred.header.frame_id = 'velodyne'
pose_list_true = list()
my_path_true = Path()
my_path_true.header.frame_id = 'velodyne'
for i in range(len(x_pred)):
pose = PoseStamped()
loc = Pose()
loc.position.x = x_pred[i]
loc.position.y = y_pred[i]
pose.pose = loc
point = Point()
point.x = x_pred[i]
point.y = y_pred[i]
point.y = 1
# pose.header.frame_id = '/odom'
# pose.header.stamp = rospy.Time.now()
pose_list_pred.append(pose)
my_path_pred.poses.append(pose)
pointArray.points.append(point)
for i in range(len(xs)):
pose = PoseStamped()
loc = Pose()
loc.position.x = xs[i]
loc.position.y = ys[i]
pose.pose = loc
# pose.header.frame_id = '/odom'
# pose.header.stamp = rospy.Time.now()
pose_list_true.append(pose)
my_path_true.poses.append(pose)
# print '-----------------------------------'
# print x_pred
# print xs[observation_length:]
# print y_pred
# print ys[observation_length:]
# print error
path_pub.publish(my_path_true)
predicted_path_pub.publish(my_path_pred)
print("About to publish point array \n")
predicted_point_pub.publish(pointArray)
pedestrianPoseList = PedestrianPoseList()
pedestrianPoseList.frameID = currentFrame + 1
pedestrianPoseList.header.stamp = rospy.Time.now()
pedestrianPoseList.header.frame_id = "velodyne"
pedestrianPose = PedestrianPose()
pedestrianPose.pedID = pedID
pedestrianPose.frameID = currentFrame + 1
pedestrianPose.x = x_pred[0]
pedestrianPose.y = y_pred[0]
pedestrianPoseList.poses.append(pedestrianPose)
predict_next_frame_pub.publish(pedestrianPoseList)
break
def transform(origin_frame, dest_frame, poseORodom):
"""Transforms poseORodom from origin_frame to dest_frame frame
Arguments:
origin_frame: the starting frame
dest_frame: the frame to trasform to
poseORodom: the Pose, PoseStamped, Odometry, or message to transform
Returns:
Pose, PoseStamped, Odometry : The transformed position
"""
tfBuffer = tf2_ros.Buffer()
trans = tfBuffer.lookup_transform(dest_frame, origin_frame, rospy.Time(), rospy.Duration(0.5))
if isinstance(poseORodom, PoseStamped):
transformed = tf2_geometry_msgs.do_transform_pose(poseORodom, trans)
return transformed
elif isinstance(poseORodom, Pose):
temp_pose_stamped = PoseStamped()
temp_pose_stamped.pose = poseORodom
transformed = tf2_geometry_msgs.do_transform_pose(temp_pose_stamped, trans)
return transformed.pose
elif isinstance(poseORodom, Odometry):
temp_pose_stamped = PoseStamped()
temp_twist_stamped = TwistStamped()
temp_pose_stamped.pose = poseORodom.pose.pose
temp_twist_stamped.twist = poseORodom.twist.twist
transformed_pose_stamped = tf2_geometry_msgs.do_transform_pose(temp_pose_stamped, trans)
# twist as points
twist_poseORodom = poseORodom.twist.twist
temp_twist_point_linear = Point()
temp_twist_point_angular = Point()
temp_twist_point_linear.x = twist_poseORodom.linear.x
temp_twist_point_linear.y = twist_poseORodom.linear.y
temp_twist_point_linear.z = twist_poseORodom.linear.z
temp_twist_point_angular.x = twist_poseORodom.angular.x
temp_twist_point_angular.y = twist_poseORodom.angular.y
temp_twist_point_angular.z = twist_poseORodom.angular.z
twist_point_linear_stamped = PointStamped(point=temp_twist_point_linear)
twist_point_angular_stamped = PointStamped(point=temp_twist_point_angular)
# points transformed
transformed_twist_point_linear_stamped = tf2_geometry_msgs.do_transform_point(
twist_point_linear_stamped, trans)
transformed_twist_point_angular_stamped = tf2_geometry_msgs.do_transform_point(
twist_point_angular_stamped, trans)
# points to twist
transformed_twist = Twist()
transformed_twist.linear.x = transformed_twist_point_linear_stamped.point.x
transformed_twist.linear.y = transformed_twist_point_linear_stamped.point.y
transformed_twist.linear.z = transformed_twist_point_linear_stamped.point.z
transformed_twist.angular.x = transformed_twist_point_angular_stamped.point.x
transformed_twist.angular.y = transformed_twist_point_angular_stamped.point.y
transformed_twist.angular.z = transformed_twist_point_angular_stamped.point.z
transformed_odometry = Odometry(header=Header(frame_id=dest_frame), child_frame_id=dest_frame,
pose=PoseWithCovariance(pose=transformed_pose_stamped.pose),
twist=TwistWithCovariance(twist=transformed_twist))
return transformed_odometry
else:
rospy.logerr("Invalid message type passed to transform()")
return None
def create_pose_stamped(pose, frame_id = 'torso_lift_link'):
m = PoseStamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
return m
def link_to_collision_object(self, link, full_linkname):
mesh_path = None
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if self.is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print(
"Element %s with geometry type %s not supported. Ignored."
% (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val)
for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(resource):
mesh_path = resource
break
if mesh_path is not None:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = pysdf.homogeneous2pose_msg(
linkpart.pose)
if not self.make_mesh(collision_object, link_pose_stamped,
mesh_path, scale):
return None
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val)
for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [float(linkpart.geometry_data['radius'])]
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(float(linkpart.geometry_data['length']),
float(linkpart.geometry_data['radius'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
return collision_object
req = GraspPlanningRequest()
req.arm_name = 'left_arm'
req.target = coll_map_res.graspable_objects[0]
req.collision_object_name = coll_map_res.collision_object_names[0]
req.collision_support_surface_name = coll_map_res.collision_support_surface_name
rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0])
grasping_result = grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('unable to find a feasable grasp, aborting')
exit(1)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('found good grasp, looking for corresponding IK')
ik_solution = find_ik_for_grasping_pose(pose_stamped)
if ik_solution is None:
exit(1)
joint_names = ('shoulder_pitch_joint',
'shoulder_pan_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_pitch_joint',
'wrist_roll_joint')
rospy.init_node('waypoint_handler')
command_pub = rospy.Publisher('trajectory_command', PoseStamped, queue_size=10)
rospy.Subscriber("/vrpn_client_node/RigidBody1/pose", PoseStamped,
optitrak_callback)
rospy.Subscriber("waypoint_array", PoseArray, waypoint_recieved)
rospy.Subscriber("drone_data_recieved", numpy_msg(Floats), drone_data_callback)
r = rospy.Rate(60)
while not rospy.is_shutdown():
print(get_current_distance())
if get_current_distance() < threshold and i < len(
waypoints.poses) and takeoff == True:
print("reached goal point")
i = i + 1
print("Assigning new waypoint")
desired_pose.pose = waypoints.poses[i]
command_pub.publish(desired_pose)
elif i == len(waypoints.poses) and i != 0:
print("Trajectory finished")
i = i + 1
desired_pose.header = current_pose.header
r.sleep()
def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(
concatenate_matrices(link.pose_world,
inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." %
(full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val) for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
test_mesh_path = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(test_mesh_path):
mesh_path = test_mesh_path
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped,
mesh_path, scale)
else:
print("ERROR: No mesh found for '%s' in element '%s'." %
(linkpart.geometry_data['uri'], full_linkname))
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(2.0 * float(linkpart.geometry_data['radius']),
float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
vehicle_id + '/cam_pose',
PoseStamped,
queue_size=2)
cam_pose = PoseStamped()
gazeboLinkstate = rospy.ServiceProxy('gazebo/get_link_state', GetLinkState)
mountConfig(header=srvheader,
mode=2,
stabilize_roll=0,
stabilize_yaw=0,
stabilize_pitch=0)
print(vehicle_type + '_' + vehicle_id + ': Gimbal control')
while not rospy.is_shutdown():
msg = MountControl()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.mode = 2
msg.pitch = gimbal_pitch_
msg.roll = gimbal_roll_
msg.yaw = gimbal_yaw_
mountCnt.publish(msg)
try:
response = gazeboLinkstate(
vehicle_type + '_' + vehicle_id + '::cgo3_camera_link',
'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s" % e
cam_pose.pose = response.link_state.pose
cam_pose_pub.publish(cam_pose)
rate.sleep()
def __init__(self):
rospy.init_node("behavior_tree")
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize the black board
self.blackboard = BlackBoard()
# How frequently do we "tic" the tree?
self.blackboard.rate = rospy.get_param('~rate', 10)
# Convert tic rate to a ROS rate
tic = rospy.Rate(self.blackboard.rate)
# Where should the DOT file be stored. Default location is $HOME/.ros/tree.dot
dotfilepath = rospy.get_param('~dotfilepath', None)
# Create the root node
BEHAVE = Sequence("BEHAVE", reset_after=True)
ACTIONS = Selector("ACTIONS")
LISTEN = MonitorTask("LISTEN", "/recognizer/output", String,
self.listen)
BEHAVE.add_child(LISTEN)
BEHAVE.add_child(ACTIONS)
FETCH = Sequence("FETCH", reset_after=True)
HOME = Sequence("HOME", reset_after=False)
TRASH = Sequence("TRASH", reset_after=False)
WATER = Sequence("WATER", reset_after=False)
# Add the two subtrees to the root node in order of priority
ACTIONS.add_child(HOME)
ACTIONS.add_child(TRASH)
ACTIONS.add_child(WATER)
ACTIONS.add_child(FETCH)
with HOME:
CHECK_HOME_DOCK = BlackboardTask("CHECK_HOME_DOCK",
self.blackboard, "docked", False)
CHECK_HOME = BlackboardTask("CHECK_HOME", self.blackboard,
"target_id", 0)
FIND_HOME = MonitorOnceTask("FIND_HOME",
"/apriltags/detections_target",
AprilTagDetections,
self.apriltag_detections)
HOME_GRIP = PublishTask("HOME_GRIP", "/grip", Int32, 60)
HOME_ARM = PublishTask("HOME_ARM", "/arm", Int32, 400)
NAV_HOME_TARGET = DynamicActionTask("NAV_HOME_TARGET",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
dock_pose = PoseStamped()
dock_pose.header.frame_id = '0'
dock_pose.header.stamp = rospy.Time.now()
p = Pose()
p.position.x = -0.02
p.position.y = 0.27
dock_pose.pose = p
DOCK_TARGET = PublishTask("DOCK_TARGET",
"apriltags/goal",
PoseStamped,
dock_pose,
6,
done_cb=self.done_dock)
HOME.add_child(CHECK_HOME_DOCK)
HOME.add_child(CHECK_HOME)
HOME.add_child(FIND_HOME)
HOME.add_child(HOME_GRIP)
HOME.add_child(HOME_ARM)
HOME.add_child(NAV_HOME_TARGET)
HOME.add_child(DOCK_TARGET)
# backup 0.6 meter if still docked
UNDOCK = Selector("UNDOCK")
CHECK_DOCK = BlackboardTask("CHECK_DOCK", self.blackboard, "docked",
False)
UNDOCK_ACTION = Sequence("UNDOCK_ACTION")
UNDOCK.add_child(CHECK_DOCK)
UNDOCK.add_child(UNDOCK_ACTION)
CLOSE_GRIP = PublishTask("CLOSE_GRIP", "/grip", Int32, 60, 2)
LOWER_ARM = PublishTask("LOWER_ARM", "/arm", Int32, 80)
backup = Twist()
backup.linear.x = -0.13
backup.linear.y = 0
backup.linear.z = 0
backup.angular.x = 0
backup.angular.y = 0
backup.angular.z = 0
UNDOCK_BACKUP = TwistTask("UNDOCK_BACKUP",
"/mobile_base/commands/velocity",
backup,
4,
rate=self.blackboard.rate,
done_cb=self.done_undock)
UNDOCK_ACTION.add_child(CLOSE_GRIP)
UNDOCK_ACTION.add_child(UNDOCK_BACKUP)
UNDOCK_ACTION.add_child(LOWER_ARM)
home_pose = Pose()
home_pose.position.x = -0.8
home_pose.position.y = 0.0
home_pose.position.z = 0.0
home_pose.orientation.z = 0.678491842963
home_pose.orientation.w = 0.734607935591
home_goal = MoveBaseGoal()
home_goal.target_pose.header.frame_id = 'map'
home_goal.target_pose.header.stamp = rospy.Time.now()
home_goal.target_pose.pose = home_pose
with TRASH:
CHECK_TRASH = BlackboardTask("CHECK_TRASH", self.blackboard,
"target_id", 2)
FIND_TRASH = MonitorOnceTask("FIND_TRASH",
"/apriltags/detections_target",
AprilTagDetections,
self.apriltag_detections)
PRESENT = SimpleActionTask("PRESENT",
"planner/move_base",
MoveBaseAction,
home_goal,
reset_after=False)
OPEN_GRIP_TRASH = PublishTask("OPEN_GRIP_TRASH", "/grip", Int32,
30)
RAISE_ARM_TRASH = PublishTask("RAISE_ARM_TRASH", "/arm", Int32,
900, 4)
DETECT_BOOL_RESET_TRASH = PublishTask("DETECT_BOOL_RESET_TRASH",
"/active", Bool, False, 0.5)
DETECT_BOOL_TRASH = PublishTask("DETECT_BOOL_TRASH", "/active",
Bool, True)
DETECT_TRASH = MonitorOnceTask("DETECT_TRASH", "/detected", Bool,
self.detect_hand)
CLOSE_GRIP_TRASH = PublishTask("CLOSE_GRIP_TRASH", "/grip", Int32,
75, 1)
LOWER_ARM_TRASH = PublishTask("LOWER_ARM_TRASH", "/arm", Int32, 80)
NAV_TRASH_TASK = DynamicActionTask("NAV_TRASH_TASK",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
RAISE_ARM_TRASH2 = PublishTask("RAISE_ARM_TRASH2", "/arm", Int32,
700, 3.5)
trash_pose = PoseStamped()
trash_pose.header.frame_id = '2'
trash_pose.header.stamp = rospy.Time.now()
p = Pose()
p.position.x = 0.12
p.position.y = 0.22
trash_pose.pose = p
TRASH_TARGET = PublishTask("TRASH_TARGET", "apriltags/goal",
PoseStamped, trash_pose, 6)
OPEN_GRIP_TRASH2 = PublishTask("OPEN_GRIP_TRASH2", "/grip", Int32,
20)
TRASH_BACK = TwistTask("TWIST",
"/mobile_base/commands/velocity",
backup,
3,
rate=self.blackboard.rate)
LOWER_ARM_TRASH2 = PublishTask("LOWER_ARM_TRASH2", "/arm", Int32,
80)
TRASH_HOME_TASK = SimpleActionTask("TRASH_HOME_TASK",
"planner/move_base",
MoveBaseAction,
home_goal,
done_cb=self.reset_fetch,
reset_after=False)
TRASH.add_child(CHECK_TRASH)
TRASH.add_child(FIND_TRASH)
TRASH.add_child(UNDOCK)
TRASH.add_child(PRESENT)
TRASH.add_child(OPEN_GRIP_TRASH)
TRASH.add_child(RAISE_ARM_TRASH)
TRASH.add_child(DETECT_BOOL_RESET_TRASH)
TRASH.add_child(DETECT_BOOL_TRASH)
TRASH.add_child(DETECT_TRASH)
TRASH.add_child(CLOSE_GRIP_TRASH)
TRASH.add_child(LOWER_ARM_TRASH)
TRASH.add_child(NAV_TRASH_TASK)
TRASH.add_child(RAISE_ARM_TRASH2)
TRASH.add_child(TRASH_TARGET)
TRASH.add_child(OPEN_GRIP_TRASH2)
TRASH.add_child(TRASH_BACK)
TRASH.add_child(LOWER_ARM_TRASH2)
TRASH.add_child(TRASH_HOME_TASK)
with WATER:
CHECK_WATER = Selector("CHECK_WATER")
CHECK_WATER3 = BlackboardTask("CHECK_WATER3", self.blackboard,
"target_id", 3)
CHECK_WATER4 = BlackboardTask("CHECK_WATER4", self.blackboard,
"target_id", 4)
CHECK_WATER.add_child(CHECK_WATER3)
CHECK_WATER.add_child(CHECK_WATER4)
FIND_WATER = MonitorOnceTask("FIND_WATER",
"/apriltags/detections_target",
AprilTagDetections,
self.apriltag_detections)
NAV_WATER_TASK = DynamicActionTask("NAV_WATER_TASK",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
WATER_ARM1 = PublishTask("WATER_ARM1", "/arm", Int32, 700)
WATER_GRIP1 = PublishTask("WATER_GRIP1", "/grip", Int32, 35, 3)
water_pose = PoseStamped()
water_pose.header.frame_id = '3'
water_pose.header.stamp = rospy.Time.now()
p = Pose()
p.position.x = 0.10
p.position.y = 0.32
water_pose.pose = p
WATER_TARGET = PublishTask("WATER_TARGET", "apriltags/goal",
PoseStamped, water_pose, 8)
WATER_GRIP2 = PublishTask("WATER_GRIP2", "/grip", Int32, 55, 1)
WATER_ARM2 = PublishTask("WATER_ARM2", "/arm", Int32, 850, 1.5)
WATER_BACKUP = TwistTask("WATER_BACKUP",
"/mobile_base/commands/velocity",
backup,
4,
rate=self.blackboard.rate)
WATER_ARM3 = PublishTask("WATER_ARM3",
"/arm",
Int32,
100,
1,
done_cb=self.target_water)
FIND_WATER2 = MonitorOnceTask("FIND_WATER2",
"/apriltags/detections_target",
AprilTagDetections,
self.apriltag_detections)
NAV_WATER_TASK2 = DynamicActionTask("NAV_WATER_TASK2",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
water_pose2 = PoseStamped()
water_pose2.header.frame_id = '4'
water_pose2.header.stamp = rospy.Time.now()
p = Pose()
p.position.x = -0.05
p.position.y = 0.42
water_pose2.pose = p
WATER_ARM32 = PublishTask("WATER_ARM32", "/arm", Int32, 380, 2)
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = -0.4
WATER_TWIST = TwistTask("WATER_TWIST",
"/mobile_base/commands/velocity",
twist,
1,
rate=self.blackboard.rate)
WATER_TARGET2 = PublishTask("WATER_TARGET2", "apriltags/goal",
PoseStamped, water_pose2, 8)
WATER_ARM35 = PublishTask("WATER_ARM35", "/arm", Int32, 340, 2)
WATER_GRIP3 = PublishTask("WATER_GRIP3", "/grip", Int32, 30, 1)
WATER_BACKUP15 = TwistTask("WATER_BACKUP15",
"/mobile_base/commands/velocity",
backup,
0.75,
rate=self.blackboard.rate)
WATER_GRIP35 = PublishTask("WATER_GRIP35", "/grip", Int32, 55, 1)
WATER_ARM4 = PublishTask("WATER_ARM4", "/arm", Int32, 710, 10)
WATER_ARM5 = PublishTask("WATER_ARM5", "/arm", Int32, 350, 2)
WATER_GRIP38 = PublishTask("WATER_GRIP38", "/grip", Int32, 40, 1)
WATER_TARGET25 = PublishTask("WATER_TARGET25", "apriltags/goal",
PoseStamped, water_pose2, 4)
WATER_GRIP4 = PublishTask("WATER_GRIP4", "/grip", Int32, 65, 1)
WATER_BACKUP2 = TwistTask("WATER_BACKUP2",
"/mobile_base/commands/velocity",
backup,
3,
rate=self.blackboard.rate)
WATER_ARM5B = PublishTask("WATER_ARM5B", "/arm", Int32, 100, 2)
WATER_HOME_TASK = SimpleActionTask("WATER_HOME_TASK",
"planner/move_base",
MoveBaseAction,
home_goal,
reset_after=False)
WATER_ARM6 = PublishTask("WATER_ARM6", "/arm", Int32, 50, 2)
WATER_GRIP5 = PublishTask("WATER_GRIP5", "/grip", Int32, 30, 1)
WATER_GRIP6 = PublishTask("WATER_GRIP6", "/grip", Int32, 10, 1)
WATER_BACKUP3 = TwistTask("WATER_BACKUP3",
"/mobile_base/commands/velocity",
backup,
2,
rate=self.blackboard.rate,
done_cb=self.reset_fetch)
WATER.add_child(CHECK_WATER)
WATER.add_child(FIND_WATER)
WATER.add_child(UNDOCK)
WATER.add_child(NAV_WATER_TASK)
WATER.add_child(WATER_ARM1)
WATER.add_child(WATER_GRIP1)
WATER.add_child(WATER_TARGET)
WATER.add_child(WATER_GRIP2)
WATER.add_child(WATER_ARM2)
WATER.add_child(WATER_BACKUP)
WATER.add_child(WATER_ARM3)
WATER.add_child(FIND_WATER2)
WATER.add_child(NAV_WATER_TASK2)
WATER.add_child(WATER_ARM32)
WATER.add_child(WATER_TWIST)
WATER.add_child(WATER_TARGET2)
WATER.add_child(WATER_ARM35)
WATER.add_child(WATER_GRIP3)
WATER.add_child(WATER_BACKUP15)
WATER.add_child(WATER_GRIP35)
WATER.add_child(WATER_ARM4)
WATER.add_child(WATER_ARM5)
WATER.add_child(WATER_GRIP38)
WATER.add_child(WATER_TARGET25)
WATER.add_child(WATER_GRIP4)
WATER.add_child(WATER_BACKUP2)
WATER.add_child(WATER_ARM5B)
WATER.add_child(WATER_HOME_TASK)
WATER.add_child(WATER_ARM6)
WATER.add_child(WATER_GRIP5)
WATER.add_child(WATER_GRIP6)
WATER.add_child(WATER_BACKUP3)
with FETCH:
FIND_WAYPOINT = MonitorOnceTask("FIND_WAYPOINT",
"/apriltags/detections_target",
AprilTagDetections,
self.apriltag_detections)
NAV_FETCH_TASK = DynamicActionTask("NAV_FETCH_TASK",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
RAISE_ARM = PublishTask("RAISE_ARM", "/arm", Int32, 780)
OPEN_GRIP = PublishTask("OPEN_GRIP", "/grip", Int32, 10, 3)
# ensure required apriltag is within view
APRILTAG_CHECK = MonitorOnceTask("APRILTAG_CHECK",
"apriltags/detections",
AprilTagDetections,
self.apriltag_check)
# move robot
target_pose = PoseStamped()
target_pose.header.frame_id = '1'
target_pose.header.stamp = rospy.Time.now()
p = Pose()
p.position.x = 0.12
p.position.y = 0.32
p.position.z = 0.0
p.orientation.w = 1.0
target_pose.pose = p
APRIL_TARGET = PublishTask("APRIL_TARGET", "apriltags/goal",
PoseStamped, target_pose, 7.5)
APRIL_GRIP = PublishTask("APRIL_GRIP", "grip", Int32, 70, 1)
RAISE_ARM2 = PublishTask("RAISE_ARM2", "/arm", Int32, 900, 1)
APRIL_BACK = TwistTask("TWIST",
"/mobile_base/commands/velocity",
backup,
3,
rate=self.blackboard.rate)
LOWER_ARM2 = PublishTask("LOWER_ARM2", "/arm", Int32, 80)
NAV_HOME_TASK = SimpleActionTask("NAV_HOME_TASK",
"planner/move_base",
MoveBaseAction,
home_goal,
reset_after=False)
RAISE_ARM3 = PublishTask("RAISE_ARM3", "/arm", Int32, 900, 5)
DETECT_BOOL = PublishTask("DETECT_BOOL", "/active", Bool, True)
DETECT_HAND = MonitorOnceTask("DETECT_HAND", "/detected", Bool,
self.detect_hand)
OPEN_GRIP2 = PublishTask("OPEN_GRIP2", "/grip", Int32, 20,
5) # delay for longer
DETECT_BOOL_RESET = PublishTask("DETECT_BOOL_RESET", "/active",
Bool, False, 0.5)
DETECT_BOOL2 = PublishTask("DETECT_BOOL2", "/active", Bool, True,
0.5)
DETECT_HAND2 = MonitorOnceTask("DETECT_HAND2", "/detected", Bool,
self.detect_hand)
CLOSE_GRIP2 = PublishTask("CLOSE_GRIP2", "/grip", Int32, 70, 2)
LOWER_ARM3 = PublishTask("LOWER_ARM3", "/arm", Int32, 80)
NAV_FETCH_TASK2 = DynamicActionTask("NAV_FETCH_TASK2",
"planner/move_base",
MoveBaseAction,
self.blackboard,
rate=self.blackboard.rate,
result_timeout=120,
reset_after=False)
RAISE_ARM4 = PublishTask("RAISE_ARM4", "/arm", Int32, 900, 3)
APRIL_TARGET2 = PublishTask("APRIL_TARGET", "apriltags/goal",
PoseStamped, target_pose, 7.5)
LOWER_ARM4 = PublishTask("LOWER_ARM4", "/arm", Int32, 800, 1)
OPEN_GRIP3 = PublishTask("OPEN_GRIP3", "/grip", Int32, 30, 1)
APRIL_BACK2 = TwistTask("TWIST",
"/mobile_base/commands/velocity",
backup,
3,
rate=self.blackboard.rate)
LOWER_ARM5 = PublishTask("LOWER_ARM5", "/arm", Int32, 80)
NAV_HOME_TASK2 = SimpleActionTask("NAV_HOME_TASK2",
"planner/move_base",
MoveBaseAction,
home_goal,
done_cb=self.reset_fetch,
reset_after=False)
FETCH.add_child(FIND_WAYPOINT)
FETCH.add_child(UNDOCK)
FETCH.add_child(NAV_FETCH_TASK)
FETCH.add_child(RAISE_ARM)
FETCH.add_child(OPEN_GRIP)
FETCH.add_child(APRIL_TARGET)
FETCH.add_child(APRIL_GRIP)
FETCH.add_child(RAISE_ARM2)
FETCH.add_child(APRIL_BACK)
FETCH.add_child(LOWER_ARM2)
FETCH.add_child(NAV_HOME_TASK)
FETCH.add_child(RAISE_ARM3)
FETCH.add_child(DETECT_BOOL)
FETCH.add_child(DETECT_HAND)
FETCH.add_child(OPEN_GRIP2)
FETCH.add_child(DETECT_BOOL2)
FETCH.add_child(DETECT_HAND2)
FETCH.add_child(CLOSE_GRIP2)
FETCH.add_child(LOWER_ARM3)
FETCH.add_child(NAV_FETCH_TASK2)
FETCH.add_child(RAISE_ARM4)
FETCH.add_child(APRIL_TARGET2)
FETCH.add_child(LOWER_ARM4)
FETCH.add_child(OPEN_GRIP3)
FETCH.add_child(APRIL_BACK2)
FETCH.add_child(LOWER_ARM5)
FETCH.add_child(NAV_HOME_TASK2)
# Display the tree before beginning execution
print "Patrol Behavior Tree"
print_tree(BEHAVE)
# Run the tree
while not rospy.is_shutdown():
BEHAVE.status = BEHAVE.run()
tic.sleep()
print_dot_tree(BEHAVE, dotfilepath)
def cb_image(self, cv_image):
"""
Runs the visual odometer with the current input image, and stacks the pose with the previously estimated poses
:param cv_image: input image for the visual odometer
:type cv_image: opencv mat
"""
if not self.active:
return
if self.thread_working:
return
self.thread_working = True
start = time.time()
# Run configured visual odometry with input image
vo_result = self.visual_odometer.get_image_and_trigger_vo(cv_image)
if vo_result is not None:
if vo_result[0] is not None:
vo_transform = vo_result[0]
if vo_transform is not None:
try:
t_vec = vo_transform.translation
z_quaternion = vo_transform.rotation
current_time = rospy.Time.now()
t = TransformStamped()
t.header.frame_id = "world"
t.child_frame_id = "axis"
# Rotate displacement vector by duckiebot rotation wrt world frame and add it to stacked displacement
t_vec = np.squeeze(
qv_multiply(self.stacked_rotation,
[t_vec.x, t_vec.y, t_vec.z]))
translation = Vector3(t_vec[0], t_vec[1], t_vec[2])
self.stacked_position = Vector3(
self.stacked_position.x + translation.x,
self.stacked_position.y + translation.y,
self.stacked_position.z + translation.z)
# Add quaternion rotation to stacked rotation to obtain the rotation transformation between world and
# duckiebot frames
quaternion = tf.transformations.quaternion_multiply(
self.stacked_rotation, [
z_quaternion.x, z_quaternion.y, z_quaternion.z,
z_quaternion.w
])
# Store quaternion and transform it to geometry message
self.stacked_rotation = quaternion
quaternion = Quaternion(quaternion[0], quaternion[1],
quaternion[2], quaternion[3])
# Broadcast transform
t.transform.translation = self.stacked_position
t.transform.rotation = quaternion
self.transform_broadcaster.sendTransform(t)
# Create odometry and Path msgs
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "world"
self.path.header = odom.header
# Set the position
odom.pose.pose = Pose(self.stacked_position, quaternion)
pose_stamped = PoseStamped()
pose_stamped.header = odom.header
pose_stamped.pose = odom.pose.pose
self.path.poses.append(pose_stamped)
odom.child_frame_id = "base_link"
# Publish the messages
self.odom_publisher.publish(odom)
self.path_publisher.publish(self.path)
rospy.logwarn("TIME: Total time: %s", time.time() - start)
rospy.logwarn(
"===================================================")
except AssertionError as e:
rospy.logwarn("Error in estimated rotation matrix")
rospy.logerr(e)
raise
if vo_result[1] is not None:
histogram_img = vo_result[1]
self.histogram_publisher.publish(histogram_img)
if vo_result[2] is not None:
mask_img = vo_result[2]
self.mask_publisher.publish(histogram_img)
self.thread_working = False
def get_next_waypoints(waypoints, current_pose, base, n, ilane):
"""Return a list of n paths ahead of the vehicle"""
frame_id = waypoints[0].header.frame_id
next_waypoints = []
for k in range(base, base + 4 * n):
wp = k % len(waypoints)
next_waypoints.append(waypoints[wp])
# frenet transform
maps_s = []
map_s = 0
maps_s.append(map_s)
map_x_prev = next_waypoints[0].pose.position.x
map_y_prev = next_waypoints[0].pose.position.y
for i in range(1, 4 * n):
map_x = next_waypoints[i].pose.position.x
map_y = next_waypoints[i].pose.position.y
map_s += distance(map_x, map_y, map_x_prev, map_y_prev)
maps_s.append(map_s)
map_x_prev = map_x
map_y_prev = map_y
# debug
# for i in range(len(maps_s)):
# rospy.loginfo('i = %d, map_s = %f' % (i, maps_s[i]))
current_x, current_y = current_pose.pose.position.x, current_pose.pose.position.y
# current_s, current_d = get_frenet(current_x, current_y, 0, next_waypoints)
# rospy.loginfo('******** s = %f, d = %f *********' % (current_s, current_d))
current_d = distance(current_x, current_y,
next_waypoints[0].pose.position.x,
next_waypoints[0].pose.position.y)
d = 0 + ilane * 4
# fits a polynomial for given paths
s_coords = [
maps_s[0], maps_s[1], maps_s[n / 2], maps_s[-3], maps_s[-2], maps_s[-1]
]
d_coords = [d, d, d, d, d, d]
f = interp1d(s_coords, d_coords, kind='cubic')
# construct lane change path
x_points = []
y_points = []
target_s = min(30.0, maps_s[-2])
target_d = d
s_add_on = 0
for i in range(n):
s_point = s_add_on + target_s / n
d_point = f(s_point)
# rospy.loginfo('s = %f, d = %f' % (s_point, d_point))
s_add_on = s_point
x_point, y_point = get_xy(s_point, d_point, maps_s, next_waypoints)
x_points.append(x_point)
y_points.append(y_point)
next_waypoints = []
next_waypoints_cloud = PointCloud()
next_waypoints_cloud.header.frame_id = frame_id
for i in range(n):
pose_stamped = PoseStamped()
pose = Pose()
pose.position.x = x_points[i]
pose.position.y = y_points[i]
pose.position.z = 0.5
pose_stamped.header.frame_id = frame_id
pose_stamped.header.seq = i
pose_stamped.pose = pose
next_waypoints.append(pose_stamped)
point = Point32()
point.x = x_points[i]
point.y = y_points[i]
point.z = 0.5
next_waypoints_cloud.points.append(point)
# rospy.loginfo('x = %f, y = %f' % (x_points[i], y_points[i]))
return next_waypoints, next_waypoints_cloud