def MovetoScanStockingRange():
#Send Baxter back to starting point to find next stocking
move_to_pose = PoseStamped()
move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
move_to_pose.pose.position=Point(
x=-0.25,
y=0.7,
z=0.3,
)
move_to_pose.pose.orientation=Quaternion(
x=-0.5,
y=0.5,
z=0.5,
w=0.5,
)
#Send PoseStamped() message to Baxter's movement function
pub_baxtermovement.publish(move_to_pose)
rospy.sleep(10)
global first_flag
first_flag = True
return
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 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 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 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 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 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 reach_position(self, x, y, z, timeout):
# set a position setpoint
pos = PoseStamped()
pos.header = Header()
pos.header.frame_id = "base_footprint"
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
yaw = math.radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
pos.pose.orientation = Quaternion(*quaternion)
# does it reach the position in X seconds?
count = 0
while count < timeout:
# update timestamp for each published SP
pos.header.stamp = rospy.Time.now()
self.pub_spt.publish(pos)
self.helper.bag_write('mavros/setpoint_position/local', pos)
if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
break
count = count + 1
self.rate.sleep()
self.assertTrue(count < timeout, "took too long to get to position")
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 publishWayPoints(xPos, yPos, angle, w_p):
print "publishing waypoint"
pub = rospy.Publisher("/move_base_simple/goal", PoseStamped)
goal = PoseStamped()
initTime = rospy.get_time()
mapWidth = globalCostMapGrid.info.width
mapOriginX = int(math.floor(globalCostMapGrid.info.origin.position.x * 20))
mapOriginY = int(math.floor(globalCostMapGrid.info.origin.position.y * 20))
goal.header = g.header
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = xPos
goal.pose.position.y = yPos
goal.pose.position.z = 0
quat = quaternion_from_euler(0, 0, angle)
goal.pose.orientation.x = quat[0]
goal.pose.orientation.y = quat[1]
goal.pose.orientation.z = quat[2]
goal.pose.orientation.w = quat[3]
valX = int(math.floor(d.way_points[w_p][0] * 20)) - mapOriginX
valY = int(math.floor(d.way_points[w_p][1] * 20)) - mapOriginY
r = rospy.Rate(0.7)
while math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2)) > 0.5:
print "d:", math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2))
finalTime = rospy.get_time()
changeTime = finalTime - initTime
if (globalCostMapGrid.data[(valX * mapWidth) + valY] > g.threshold) or (changeTime > 120):
break
else:
pub.publish(goal)
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 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 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 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 MovetoScanRange():
#Combine position and orientation in a PoseStamped() message
move_to_pose = PoseStamped()
move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
move_to_pose.pose.position=Point(
x=-0.25, #-0.25
y=0.7, #0.7
z=0.3, #0.3
)
move_to_pose.pose.orientation=Quaternion(
x=-0.5,
y=0.5,
z=0.5,
w=0.5,
)
#Send PoseStamped() message to Baxter's movement function
pub_baxtermovement.publish(move_to_pose)
rospy.sleep(2)
global first_flag
first_flag = True
return
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 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 test_attctl(self):
# set some attitude and thrust
att = PoseStamped()
att.header = Header()
att.header.frame_id = "base_footprint"
att.header.stamp = rospy.Time.now()
quaternion = quaternion_from_euler(0.15, 0.15, 0)
att.pose.orientation = Quaternion(*quaternion)
throttle = Float64()
throttle.data = 0.6
# does it cross expected boundaries in X seconds?
count = 0
timeout = 120
while count < timeout:
# update timestamp for each published SP
att.header.stamp = rospy.Time.now()
self.pub_att.publish(att)
self.pub_thr.publish(throttle)
self.rate.sleep()
if (self.local_position.pose.position.x > 5
and self.local_position.pose.position.z > 5
and self.local_position.pose.position.y < -5):
break
count = count + 1
self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set")
self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set")
self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
self.assertTrue(count < timeout, "took too long to cross boundaries")
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 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 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 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 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 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 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
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 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 callback(data):
p = PoseStamped()
p.header = data.header
p.pose.position = data.transform.translation
p.pose.orientation = data.transform.rotation
pub.publish(p);
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 picknplace():
# Initialize the planning scene interface.
p = PlanningSceneInterface("base")
# Connect the arms to the move group.
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
# Create baxter_interface limb instance.
leftarm = baxter_interface.limb.Limb('left')
# Create baxter_interface gripper instance.
leftgripper = baxter_interface.Gripper('left')
leftgripper.open()
# Define the joints for the positions.
jts_both = [
'left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1',
'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0',
'right_w1', 'right_w2'
]
jts_right = [
'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1',
'right_w2'
]
jts_left = [
'left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1',
'left_w2'
]
pos1 = [
-1.4580487388850858, 1.627553615946424, 0.07669903939427068,
-0.3539660668045592, -1.9155585088719105, -1.4124128104454947,
-0.6438884357149024, 1.7238109084167481, 1.7169079948791506,
0.36930587426147465, -0.33249033539428713, -1.2160632682067871,
1.668587600115967, -1.810097327636719
]
pos2 = [
-0.949534106616211, 1.4994662184448244, -0.6036214393432617,
-0.7869321432861328, -2.4735440176391603, -1.212228316241455,
-0.8690001153442384, 1.8342575250183106, 1.8668546167236328,
-0.45674277907104494, -0.21667478604125978, -1.2712865765075685,
1.7472041154052735, -2.4582042097778323
]
lpos1 = [
-1.4580487388850858, 1.627553615946424, 0.07669903939427068,
-0.3539660668045592, -1.9155585088719105, -1.4124128104454947,
-0.6438884357149024
]
lpos2 = [
-0.949534106616211, 1.4994662184448244, -0.6036214393432617,
-0.7869321432861328, -2.4735440176391603, -1.212228316241455,
-0.8690001153442384
]
rpos1 = [
1.7238109084167481, 1.7169079948791506, 0.36930587426147465,
-0.33249033539428713, -1.2160632682067871, 1.668587600115967,
-1.810097327636719
]
rpos2 = [
1.8342575250183106, 1.8668546167236328, -0.45674277907104494,
-0.21667478604125978, -1.2712865765075685, 1.7472041154052735,
-2.4582042097778323
]
placegoal = PoseStamped()
placegoal.header.frame_id = "base"
placegoal.header.stamp = rospy.Time.now()
placegoal.pose.position.x = 0.55
placegoal.pose.position.y = 0.28
placegoal.pose.position.z = 0
placegoal.pose.orientation.x = 1.0
placegoal.pose.orientation.y = 0.0
placegoal.pose.orientation.z = 0.0
placegoal.pose.orientation.w = 0.0
# Define variables.
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
# The distance from the zero point in Moveit to the ground is 0.903 m.
# The value is not allways the same. (look in Readme)
center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
pressure_ok = 0
j = 0
k = 0
start = 1
locs_x = []
# Initialize a list for the objects and the stacked cubes.
objlist = [
'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
'obj09', 'obj10', 'obj11'
]
boxlist = [
'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
'box09', 'box10', 'box11'
]
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
# Move both arms to start state where the right camera looks for objects.
g.moveToJointPosition(jts_both,
pos1,
max_velocity_scaling_factor=1,
plan_only=False)
time.sleep(0.5)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
# Loop to continue pick and place until all objects are cleared from table.
while locs_x or start:
# Only for the start.
if start:
start = 0
# Receive the data from all objects from the topic "detected_objects".
temp = rospy.wait_for_message("detected_objects", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
# Adds the data from the objects.
for i in range(len(locs)):
locs_x.append(locs[i].position.x)
locs_y.append(locs[i].position.y)
orien.append(locs[i].position.z * pi / 180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected locations for same objects.
ind_rmv = []
for i in range(0, len(locs)):
if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
ind_rmv.append(i)
continue
for j in range(i, len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 +
(locs_y[i] - locs_y[j])**2) < 0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Do the task only if there are still objects on the table.
if locs_x:
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
# Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(
zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Initialize the data of the biggest object on the table.
xn = locs_x[0]
yn = locs_y[0]
zn = -0.167
thn = orien[0]
sz = size[0]
if thn > pi / 4:
thn = -1 * (thn % (pi / 4))
# Add the detected objects into the planning scene.
#for i in range(1,len(locs_x)):
#p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
# Add the stacked objects as collision objects into the planning scene to avoid moving against them.
#for e in range(0, k):
#p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
if k > 0:
p.attachBox(boxlist[0],
0.07,
0.07,
0.0275 * k,
placegoal.pose.position.x,
placegoal.pose.position.y,
center_z_cube,
'base',
touch_links=['cubes'])
p.waitForSync()
# Move both arms to the second position.
g.moveToJointPosition(jts_both,
pos2,
max_velocity_scaling_factor=1,
plan_only=False)
# Initialize the pickgoal.
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
# Move to the approach pickgoal (5 cm to pickgoal).
pickgoal.pose.position.z = zn + 0.05
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
pickgoal.pose = rotate_pose_msg_by_euler_angles(
pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
pickgoal.pose.position.z = zn
# Move to the pickgoal.
gl.moveToPose(pickgoal,
"left_gripper",
max_velocity_scaling_factor=0.3,
plan_only=False)
leftgripper.close()
attempts = 0
pressure_ok = 1
# If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects
# 0-46: not attached to an object and 47-175: attached to an object\n176+: error
while (leftgripper.vacuum_sensor() < 30 and pressure_ok == 1):
time.sleep(0.04)
attempts += 1
if (attempts > 50):
leftgripper.open()
pressure_ok = 0
print("----->pressure is to low<-----")
# Move back to the approach pickgoal.
pickgoal.pose.position.z = zn + 0.05
gl.moveToPose(pickgoal,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Move both arms to position 1.
g.moveToJointPosition(jts_both,
pos1,
max_velocity_scaling_factor=1,
plan_only=False)
if pressure_ok:
# Define the approach placegoal.
# Increase the height of the tower every time by 2.75 cm.
placegoal.pose.position.z = -0.145 + (k * 0.0275) + 0.08
# Move to the approach pickgoal.
gl.moveToPose(placegoal,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Define the placegoal.
placegoal.pose.position.z = -0.145 + (k * 0.0275)
gl.moveToPose(placegoal,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
leftgripper.open()
while (leftgripper.vacuum_sensor() > 20):
time.sleep(0.01)
k += 1
# Move to the approach placegoal.
placegoal.pose.position.z = -0.145 + (k * 0.0275) + 0.08
gl.moveToPose(placegoal,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Move left arm to start position pos1.
gl.moveToJointPosition(jts_left,
lpos1,
max_velocity_scaling_factor=1,
plan_only=False)
pr.disable()
sortby = 'cumulative'
ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
p.clear()
import random
import math
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from flipkart.msg import detection
from armf import armtakeoff
rospy.init_node('navigation', anonymous=True)
xpixel = 640
ypixel = 480
fovx = 1.047
bbox = detection()
oldbbox = detection()
setpoint = PoseStamped()
current_position = PoseStamped()
vel = TwistStamped()
i = 0
def cvcallback(cv_msg):
global bbox
bbox = cv_msg
def callback_pos(pos):
global current_position
current_position = pos
#!/usr/bin/env python
import rospy
import tf
from geometry_msgs.msg import PoseStamped
from std.msg import string
if __name__=='__main__':
rospy.init_node('hoge')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
target_pose = TransformSamped()
starget_pose.header.frame_id = "/r_gripper_tool_frame"
target_pose.pose.
source_pose = PoseStamped()
source_pose.header.frame_id = "/base_footprint"
source_pose.header.stamp = rospy.Time.now()
source_pose.pose.
try:
target_frame =
source_frame =
listener = tf.TransformListener()
listener.waitForTransform(target_frame, source_frame, rospy.Time(), rospy.Duration(4.0))
tf = listener.transformPose(source_frame, target_frame, rospy.Time(0))
pub = rospy.Publisher('endcrd', PoseStamped, queue_size=1)
pub.publish(tf)
import sys
import copy
import rospy
import random
import math
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from armf import armtakeoff
rospy.init_node('navigation', anonymous=True)
i = 0
current_position = PoseStamped()
def callback_pos(pos):
global current_position
current_position = pos
def main():
while True:
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
callback_pos)
publish_vel = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
vel = TwistStamped()
# pose.pose.position.x = 0.7
def listener():
global current_state
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.loginfo("start listenning")
rate = rospy.Rate(100)
rate.sleep()
#rospy.Subscriber("/mavros/imu/data", Imu, callback)
#rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, callback)
rospy.Subscriber("/uav2/mavros/state", State, callback)
arming_client = rospy.ServiceProxy("/uav2/mavros/cmd/arming", CommandBool)
pub = rospy.Publisher("/uav2/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
pub1 = rospy.Publisher("/uav2/mavros/setpoint_attitude/attitude",
PoseStamped,
queue_size=10)
#pub=rospy.Publisher("/uav2/mavros/setpoint_position/local",PoseStamped,queue_size=10)
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
pose = Float64()
pose.data = 0.6
att = PoseStamped()
att.pose.orientation.x = 0.5
att.pose.orientation.y = 0.0
att.pose.orientation.z = 0.0
att.pose.orientation.w = 0.866
'''
pose=PoseStamped()
pose.pose.position.x=0
pose.pose.position.y=0
pose.pose.position.z=1
'''
rospy.loginfo("running")
set_mode_client = rospy.ServiceProxy("/uav2/mavros/set_mode", SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
pub.publish(pose)
pub1.publish(att)
#rospy.loginfo(current_state.mode)
if ((current_state.mode != "OFFBOARD")
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(current_state.mode)
rospy.loginfo("1")
if (set_mode_client.call(offb_set_mode).success):
#if(set_mode_client.call(offb_set_mode)==True and offb_set_mode.response.success==True):
rospy.loginfo("offbrd enabled")
last_request = rospy.get_rostime().secs
else:
if ((current_state.armed == False)
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo("2")
if (arming_client.call(arm_cmd).success):
rospy.loginfo("armed")
last_request = rospy.get_rostime().secs
rate.sleep()
#!/usr/bin/env python
import rospy
import mavros_msgs
from mavros_msgs import srv
from geometry_msgs.msg import PoseStamped, TwistStamped, Quaternion
from mavros_msgs.msg import State
import time
import math
goal_pose = PoseStamped() #Possicao que voce deseja ir
local = PoseStamped() #capta a posicao q vc esta
drone_pose = PoseStamped(
) #variavel que recebe a posicao q esta e usaremos para comparacoes
current_state = State() #recebe o estado da maquina
goal_rotation = Quaternion()
#set_posicao recebe de parametros a posicao que deseja ir e publicara
#rospy.init_node('Vel_Control_Node', anonymous = True)
#rate = rospy.Rate(10)
# class Circle:
# def __init__(self):
# self.local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 100)
#
# self.local_atual = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_callback)
# self.arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# def callback(self, data)
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import math
from pyquaternion import Quaternion
import tf
import sys
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(0, -math.pi / 2,
math.pi / 2)
q = Quaternion([quaternion[3], quaternion[0], quaternion[1], quaternion[2]])
def vins_callback(data):
local_pose.pose.position.x = data.pose.pose.position.x
local_pose.pose.position.y = data.pose.pose.position.y
local_pose.pose.position.z = data.pose.pose.position.z
q_ = Quaternion([
data.pose.pose.orientation.w, data.pose.pose.orientation.x,
data.pose.pose.orientation.y, data.pose.pose.orientation.z
])
q_ = q_ * q
local_pose.pose.orientation.w = q_[0]
local_pose.pose.orientation.x = q_[1]
local_pose.pose.orientation.y = q_[2]
local_pose.pose.orientation.z = q_[3]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01) # 0.01
right_arm.set_goal_orientation_tolerance(0.1) # 0.05
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.45 # 0.75
# DAVES ADDED
table_offset_x = 0.30 # meters (centemeters / 100)
table_offset_y = -0.30 # meters (positive = left)
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.26 + table_offset_x
table_pose.pose.position.y = table_offset_y
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.21 + table_offset_x
box1_pose.pose.position.y = -0.1 + table_offset_y
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.19 + table_offset_x
box2_pose.pose.position.y = 0.15 + table_offset_y
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2 + table_offset_x
target_pose.pose.position.y = table_offset_y
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def list_to_pose_stamped(self, pose_list, target_frame):
pose_msg = PoseStamped()
pose_msg.pose = self.list_to_pose(pose_list)
pose_msg.header.frame_id = target_frame
pose_msg.header.stamp = rospy.Time.now()
return pose_msg
def convert_pose():
pose = PoseStamped()
pose.pose.position.z = tools.get_pose(manipulator)[2]
return pose
def publish_state(self):
pose = PoseStamped()
pos, ori = self.get_pose()
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = ori
self._state_pub.publish(pose)
def special_case(self):
# we hard code turning situation here
# not the bast solution, but an achievable one at the moment
if self.last_node == 0:
self.normal = True
return
this_ang = self.tag_ang_init[self.guid_path[self.last_node]]
if this_ang != 180:
self.get_goal = False
self.target_global = None
while self.target_global is None:
self.target_global = PoseStamped()
self.target_global.pose.position.x = 5 * math.cos(
math.radians(this_ang))
self.target_global.pose.position.y = 5 * math.cos(
math.radians(this_ang))
self.target_global = self.transform_pose(
self.target_global.pose, self.map_frame, self.bot_frame)
self.target_global.header.frame_id = self.map_frame
self.normal = True
self.pub_robot_go.publish(True)
rospy.sleep(5)
self.get_goal = True
else:
self.get_goal = False
# 180 degree turn, stage 1, tie up harness
while self.joint_ang > math.radians(5):
# play sound on robot
self.pub_robot_sound('tie_up')
rospy.sleep(4)
# 180 degree turn, stage 2, turn 180
turn_goal = None
print("turn_goal")
while turn_goal is None:
turn_goal = PoseStamped()
turn_goal.pose.position.x = -5
turn_goal_bot = turn_goal
turn_goal_pid = turn_goal
turn_goal_pid.pose.position.x = -0.05
turn_goal_pid.pose.position.y = -0.005
turn_goal = self.transform_pose(turn_goal.pose, self.map_frame,
self.bot_frame)
turn_goal_dummy = turn_goal
self.pub_robot_go.publish(True)
print("after")
while math.fabs(
math.atan2(
turn_goal_bot.pose.position.y,
turn_goal_bot.pose.position.x)) >= math.radians(90):
print("pub turn goal")
self.pub_pid_goal.publish(
self.transform_pose(turn_goal_pid.pose, self.map_frame,
self.bot_frame))
rospy.sleep(0.1)
turn_goal_bot = self.transform_pose(turn_goal.pose,
self.bot_frame,
self.map_frame)
print(
math.fabs(
math.atan2(turn_goal_bot.pose.position.y,
turn_goal_bot.pose.position.x)))
self.pub_robot_go.publish(False)
# 180 degree turn, stage 3, release harness
while self.joint_ang < math.radians(45):
# play sound on robot
self.pub_robot_sound('release')
rospy.sleep(4)
self.normal = True
self.get_goal = True
do()
def amcl_callback(msg):
global coords
coords = msg
rospy.init_node('Navigator') #Задание ноды
vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Скорость робота, отправляемая нодой
move_goal = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10) #Конечная цель, координаты
sub = rospy.Subscriber('/base_scan', LaserScan, laser_callback)
pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_callback)
min_pose = PoseStamped() #Отправляемые
start_new_thread(command, ())
while not rospy.is_shutdown():
msg = Twist()
if state == 2:
msg = turn('right')
if state == 1:
msg = turn('left')
if state == 0:
msg = go_along()
if (min_ranges > sum_ranges) or (min_ranges == 0):
min_ranges = sum_ranges
# print(min_ranges)
def getWaypoints(initPos, goalPos, grid):
startCell = convertPointToCell(initPos, grid.origin, grid.resolution)
goalCell = convertPointToCell(goalPos, grid.origin, grid.resolution)
#Logic that gets the robot out of an obstacle (failure recovery)
if grid.getCellValue(startCell) == CellType.Obstacle:
print "The robot is inside the obstacle! Trying to find the shortest way out..."
cellTypes = set()
cellTypes.add(CellType.Empty)
cellTypes.add(CellType.Unexplored)
pathOutOfObstacle = PathFinder.findPathToCellWithValueClosestTo(grid, startCell, cellTypes, goalCell)
startCell = pathOutOfObstacle.pop(len(pathOutOfObstacle) - 1)
pathFinder = PathFinder(startCell, goalCell)
if (startCell == goalCell):
pathFinder.waypoints.append(goalCell)
else:
aStarDone = False
while not aStarDone:
aStarDone = pathFinder.runAStarIteration(grid)
publishGridCells(expanded_cell_pub, pathFinder.expanded, grid.resolution, grid.cellOrigin)
#Convert frontier queue to the frontier set
frontierCells = set()
for tuple in pathFinder.frontier.elements:
cell = tuple[1]
if cell not in pathFinder.expanded and cell not in frontierCells:
frontierCells.add(cell)
publishGridCells(frontier_cell_pub, frontierCells, grid.resolution, grid.cellOrigin)
pathFinder.findPath()
#if robot was at an obstacle cell before, then prepend the path out of the obstacle
try:
pathOutOfObstacle
except NameError:
pass
else:
pathOutOfObstacle.extend(pathFinder.path)
pathFinder.path = pathOutOfObstacle
publishGridCells(path_cell_pub, pathFinder.path, grid.resolution, grid.cellOrigin)
pathFinder.calculateWaypoints()
#convert the waypoints to the trajectory offsets:
waypoints = Path()
waypoints.poses = []
for cell in pathFinder.waypoints:
poseObj = PoseStamped()
poseObj.pose.position = convertCellToPoint(cell, grid.cellOrigin, grid.resolution)
waypoints.poses.append(poseObj)
return waypoints
def publishTarget(self, trans, rot):
msg = PoseStamped()
msg.pose.position = Point(*tuple([x * 0.001 for x in trans]))
msg.pose.orientation = Quaternion(*rot)
self.move_pub.publish(msg)
# Pick the beer
if grasping_client.pick(pringles, grasps, 1):
break
rospy.logwarn("Grasping failed.")
# Goto grasping position
grasping_client.goto_plan_grasp()
# Point the head at the stuff we want to pick
head_action.look_at(0.9, 0.1, table_height + .1, "base_link")
# Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = beer.primitive_poses[0]
pose.pose.position.z += 0.01
pose.header.frame_id = beer.header.frame_id
if grasping_client.place(beer, pose, gripper=0):
break
rospy.logwarn("Placing failed.")
grasping_client.goto_plan_grasp()
# Point the head at the stuff we want to pick
head_action.look_at(0.9, -0.1, table_height + .1, "base_link")
# Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
def tracking(self, event):
if not self.normal:
return
st = rospy.Time.now()
#rospy.loginfo("tracking loop")
if self.target_global is None:
rospy.logerr("%s : no goal" % rospy.get_name())
return
else:
#rospy.loginfo("%s :have seen goal" % rospy.get_name())
pass
#print("fuckkckckc why???")
#print(self.target_global)
end_p = self.transform_pose(self.target_global.pose, self.bot_frame,
self.map_frame)
#end_p = self.target_global
#end_p.header.frame_id = self.map_frame
if end_p is None:
return
end_p.header.frame_id = self.bot_frame
start_p = PoseStamped()
start_p.pose.position.x = 0
start_p.pose.position.y = 0
start_p.pose.position.z = 0
start_p.header.frame_id = self.bot_frame
#start_p = self.transform_pose(start_p.pose, self.map_frame, self.bot_frame)
if start_p is None:
return
req_path = GetPlanRequest()
req_path.start = start_p
req_path.goal = end_p
oe = end_p
for i in range(self.search_max):
try:
res_path = self.req_path_srv(req_path)
self.last_plan = res_path
#rospy.loginfo("%s : plan success" % rospy.get_name())
break
except:
rospy.logerr("%s : path request fail" % rospy.get_name())
if self.last_plan is None:
return
res_path = self.last_plan
r = np.linalg.norm([oe.pose.position.x, oe.pose.position.y])
theta = math.atan2(oe.pose.position.y, oe.pose.position.x)
theta += (-1)**(i) * (i + 1) * self.search_angle
end_p.pose.position.x = r * math.cos(theta)
end_p.pose.position.y = r * math.sin(theta)
self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1]))
self.pursuit.set_path(res_path.plan)
goal = self.pursuit.get_goal(start_p)
if goal is None:
rospy.logwarn("goal reached, to next goal")
self.target_global = None
self.set_lane(True)
return
# print("trace", goal)
goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame)
goal.header.frame_id = self.map_frame
self.pub_goal_marker.publish(self.to_marker(goal))
self.pub_pid_goal.publish(goal)
et = rospy.Time.now()
#path= np.genfromtxt('/home/ubuntu/Desktop/PATH/pathshort.csv', delimiter=',')
def go(msg):
posestamped= PoseStamped()
publish_heading_control.publish(posestamped)
rospy.logwarn("Current Pose Initialized")
publish_path.publish(path)
if __name__ == '__main__':
rospy.init_node('Path_Visualizer', anonymous=True)
patharray= np.genfromtxt('/home/ubuntu/Desktop/PATH/path.csv', delimiter=',')
publish_heading_control = rospy.Publisher('/Heading_Control', PoseStamped, queue_size = 1)
publish_path = rospy.Publisher('/excelpath', Path, queue_size = 1)
pose = PoseStamped()
pose.header.frame_id = "my_frame"
path = Path ()
path.header.stamp = rospy.Time.now()
path.header.frame_id = "my_frame"
for i in patharray:
pose.header.stamp = rospy.Time.now()
#pose.pose.position.x = patharray[i][1]
#pose.pose.position.y = patharray[i][2]
pose.pose.position.x = i
pose.pose.position.y = i
path.poses.append(pose)
rospy.logwarn(str(pose))
global sub
sub = rospy.Subscriber('rosout', Log , go)
rospy.spin()
def do_playback_keyframe_demo(self, goal):
rospy.loginfo("Received playback demo")
## input: keyframe demo goal has bagfile name and eef_only bool
self.start_time = time.time()
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
tfBroadcaster = tf2_ros.TransformBroadcaster()
# Check if the bag file is valid
# Example path if necessary
bag_path = os.path.expanduser(goal.bag_file_name)
if (not os.path.isfile(bag_path)):
error_msg = "Playback bagfile does not exist: %s" % bag_path
self.server.set_aborted(self.result, error_msg)
rospy.logerr(error_msg)
return
with rosbag.Bag(bag_path) as bag:
self.bag = bag
# Check the number of playback - and warn/stop if we have more than X number of keyframes
if self.bag.get_message_count() > self.KEYFRAME_THRESHOLD:
error_msg = "Playback Keyframe Demo aborted due to too many frames: %d" % self.bag.get_message_count(
)
self.server.set_aborted(self.result, error_msg)
rospy.logerr(error_msg)
return
keyframe_count = 0
# Check if we have a gripper topic. If so add it to playback
all_topics = self.bag.get_type_and_topic_info().topics.keys()
playback_topics = [goal.target_topic]
GRIPPER_TOPIC = 'gripper/stat'
gripper_topics = [x for x in all_topics if GRIPPER_TOPIC in x]
playback_topics.extend(gripper_topics)
OBJECT_LOCATION_TOPIC = "object_location"
playback_topics.append(OBJECT_LOCATION_TOPIC)
gripper_msgs = dict()
self.gripper_pos = dict()
while not self.server.is_preempt_requested():
# Cycle through the topics and messages and store them into list for ordering
all_messages = dict()
for topic, msg, t in self.bag.read_messages(
topics=playback_topics):
if topic not in all_messages:
all_messages[topic] = []
all_messages[topic].append(msg)
# Pull out the playback topic
playback_list = all_messages[goal.target_topic]
for gripper_topic in gripper_topics: # either one or two grippers
gripper_msgs[gripper_topic] = all_messages[gripper_topic]
# Actually set the gripper value
pos = gripper_msgs[gripper_topic][0].requested_position
self.gripper_helper(gripper_topic, pos)
for msg_count in xrange(len(playback_list)):
msg = playback_list[msg_count]
if goal.zero_marker:
zeroMarker = None
if not OBJECT_LOCATION_TOPIC in all_messages:
rospy.logerr(
"Playback specified a zero marker but no object locations were found in keyframe #{}"
.format(msg_count))
self.server.set_aborted(
text="Object locations missing")
return
for i, location in enumerate(
all_messages[OBJECT_LOCATION_TOPIC]
[msg_count].objects):
if location.label == goal.zero_marker:
zeroMarker = location
break
if not zeroMarker:
rospy.logerr(
"Specified zero marker not found in .bag file in keyframe #{}"
.format(msg_count))
self.server.set_aborted(
text="Zero marker not found in .bag file")
return
try:
currentZero = tfBuffer.lookup_transform(
"map",
goal.zero_marker,
rospy.Time(0),
timeout=rospy.Duration(5)).transform
except tf2_ros.LookupException:
rospy.logerr(
"Specified label \"{}\" not found in current scene. Disable locate objects to play back absolute keyframe positions."
.format(zeroMarker.label))
self.server.set_aborted(
text="Specified label not found")
return
rospy.loginfo(
"Using zero marker \"{}\" (prob: {:.1%}) with position (x: {:.2f}, y: {:.2f}, z: {:.2f})"
.format(zeroMarker.label, zeroMarker.probability,
zeroMarker.pose.position.x,
zeroMarker.pose.position.y,
zeroMarker.pose.position.z))
rospy.loginfo(
"Found corresponding zero marker in current frame (x: {:.2f}, y: {:.2f}, z: {:.2f})"
.format(currentZero.translation.x,
currentZero.translation.y,
currentZero.translation.z))
else:
rospy.loginfo(
"No zero marker passed for keyframe #{}".format(
msg_count))
# Check if we need to convert the msg into joint values
if goal.eef_only:
# Ask the arm planner to plan for that joint target from current position
pt = Pose(msg.position, msg.orientation)
if zeroMarker:
# Remap the point to the global map frame for adjustment
baseLinkToMap = tfBuffer.lookup_transform(
"map",
"base_link",
rospy.Time(0),
timeout=rospy.Duration(5))
mapToBaseLink = tfBuffer.lookup_transform(
"base_link",
"map",
rospy.Time(0),
timeout=rospy.Duration(5))
ptAbsolute = tf2_geometry_msgs.do_transform_pose(
PoseStamped(pose=pt), baseLinkToMap)
# Compensate for new zero marker position
ptAbsolute.pose.position.x += (
currentZero.translation.x -
zeroMarker.pose.position.x)
ptAbsolute.pose.position.y += (
currentZero.translation.y -
zeroMarker.pose.position.y)
ptAbsolute.pose.position.z += (
currentZero.translation.z -
zeroMarker.pose.position.z)
pt = tf2_geometry_msgs.do_transform_pose(
ptAbsolute, mapToBaseLink).pose
rospy.loginfo(
"Moving EEF to adjusted position: (x: {:.2f}, y: {:.2f}, z: {:.2f})"
.format(pt.position.x, pt.position.y,
pt.position.z))
plan = self.arm_planner.plan_poseTargetInput(pt)
else:
# Pull out the joint values for the arm from the message
pts = self._get_arm_joint_values(msg)
# Ask the arm planner to plan for that joint target from current position
plan = self.arm_planner.plan_jointTargetInput(pts)
# Check if we have a valid plan
if plan == None or len(plan.joint_trajectory.points) < 1:
print "Error: no plan found"
else:
rospy.loginfo("Executing Keyframe: %d" %
keyframe_count)
self.sendPlan(plan)
keyframe_count += 1
# Execute Gripper if needed
for gripper_topic in gripper_topics:
pos = gripper_msgs[gripper_topic][
msg_count].requested_position
if abs(pos - self.gripper_pos[gripper_topic]
) > self.GRIPPER_THRESHOLD:
# Actually set the gripper value
self.gripper_helper(gripper_topic, pos)
self.result.time_elapsed = rospy.Duration.from_sec(
time.time() - self.start_time)
complete_msg = "Playback Keyframe Demo completed successfully"
self.server.set_succeeded(self.result, complete_msg)
rospy.loginfo(complete_msg)
return
self.result.time_elapsed = rospy.Duration.from_sec(time.time() -
start_time)
self.server.set_preempted(self.result,
"playback keyframe demo preempted")
print 'preempted'
return
def pick_aruco(self, string_operation):
if string_operation == "place":
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
self.pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
return
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
self.ps = PoseStamped()
self.ps.pose.position = aruco_pose.pose.position
self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
self.ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
self.ps.header.frame_id,
rospy.Time(0))
self.aruco_ps = do_transform_pose(self.ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
self.pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
self.pick_g.object_pose.pose.position = self.aruco_ps.pose.position
self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g))
self.pick_g.object_pose.header.frame_id = 'base_footprint'
self.pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(self.pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(self.pick_g))
self.pick_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
def go(msg):
posestamped= PoseStamped()
publish_heading_control.publish(posestamped)
rospy.logwarn("Current Pose Initialized")
publish_path.publish(path)
def get_command(self, msg):
'''
Publishes the wrench for this instant.
(Note: this is called get_command because it used to be used for
getting the actual thruster values, but now it is only being
used for getting the wrench which is then later mapped elsewhere).
'''
if self.p_ref is None:
return # C3 is killed
# Compute timestep from interval between this message's stamp and last's
if self.last_odom is None:
self.last_odom = msg
else:
self.timestep = (msg.header.stamp - self.last_odom.header.stamp).to_sec()
self.last_odom = msg
# ROS read-in
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
lin_vel_body = msg.twist.twist.linear
ang_vel_body = msg.twist.twist.angular
# ROS unpack
self.position = np.array([position.x, position.y])
self.orientation = np.array([orientation.x, orientation.y, orientation.z, orientation.w])
# Frame management quantities
R = np.eye(3)
R[:2, :2] = trns.quaternion_matrix(self.orientation)[:2, :2]
y = trns.euler_from_quaternion(self.orientation)[2]
# More ROS unpacking, converting body frame twist to world frame lin_vel and ang_vel
self.lin_vel = R.dot(np.array([lin_vel_body.x, lin_vel_body.y, lin_vel_body.z]))[:2]
self.ang_vel = R.dot(np.array([ang_vel_body.x, ang_vel_body.y, ang_vel_body.z]))[2]
# Convert body PD gains to world frame
kp = R.dot(self.kp_body).dot(R.T)
kd = R.dot(self.kd_body).dot(R.T)
# Compute error components (reference - true)
p_err = self.p_ref - self.position
y_err = trns.euler_from_quaternion(trns.quaternion_multiply(
self.q_ref, trns.quaternion_inverse(self.orientation)))[2]
v_err = self.v_ref - self.lin_vel
w_err = self.w_ref - self.ang_vel
#Adjust kp_body and kd_body based on p_err if heavy_pid was set
if (self.heavy_pid == True) and (abs(p_err[0]) < self.x_thresh) and (abs(p_err[1]) < self.y_thresh) and (abs(y_err) < self.yaw_thresh):
print("threshold hit")
self.kp_body = self.kp_body_orig
self.kd_body = self.kd_body_orig
self.heavy_pid = False
self.movement_finished = True
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
# Compute "anticipation" feedforward based on the boat's inertia
inertial_feedforward = np.concatenate(
(self.a_ref, [self.aa_ref])) * [self.mass_ref, self.mass_ref, self.inertia_ref]
# Compute the "learning" matrix
drag_regressor = np.array([
[
self.lin_vel[0] * np.cos(y)**2 + self.lin_vel[1] * np.sin(y) * np.cos(y),
self.lin_vel[0] / 2 - (self.lin_vel[0] * np.cos(2 * y)) / 2 - (self.lin_vel[1] * np.sin(2 * y)) / 2,
-self.ang_vel * np.sin(y),
-self.ang_vel * np.cos(y),
0
],
[
self.lin_vel[1] / 2 - (self.lin_vel[1] * np.cos(2 * y)) / 2 + (self.lin_vel[0] * np.sin(2 * y)) / 2,
self.lin_vel[1] * np.cos(y)**2 - self.lin_vel[0] * np.cos(y) * np.sin(y),
self.ang_vel * np.cos(y),
-self.ang_vel * np.sin(y),
0
],
[
0,
0,
self.lin_vel[1] * np.cos(y) - self.lin_vel[0] * np.sin(y),
- self.lin_vel[0] * np.cos(y) - self.lin_vel[1] * np.sin(y),
self.ang_vel
]])
# wrench = PD + feedforward + I + adaptation
if self.only_PD:
wrench = (kp.dot(err)) + (kd.dot(errdot))
self.drag_effort = [0, 0, 0]
self.dist_est = [0, 0, 0]
else:
self.drag_effort = np.clip(drag_regressor.dot(self.drag_est), -self.drag_limit, self.drag_limit)
wrench = (kp.dot(err)) + (kd.dot(errdot)) + inertial_feedforward + self.dist_est + self.drag_effort
# Update disturbance estimate, drag estimates
if self.learn and (npl.norm(p_err) < self.learning_radius):
self.dist_est = np.clip(self.dist_est + (self.ki * err * self.timestep), -
self.dist_limit, self.dist_limit)
self.drag_est = self.drag_est + (self.kg * (drag_regressor.T.dot(err + errdot)) * self.timestep)
# Update model reference for the next call
if not self.use_external_tgen:
self.increment_reference()
# convert wrench to body frame
wrench_body = R.T.dot(wrench)
# NOT NEEDED SINCE WE ARE USING A DIFFERENT NODE FOR ACTUAL THRUSTER MAPPING
# # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
# B = np.concatenate((R.dot(self.thruster_directions.T), R.dot(self.lever_arms.T)))
# B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
# command = self.thruster_mapper(wrench, B_3dof)
# Give wrench to ROS
wrench_msg = WrenchStamped()
wrench_msg.header.frame_id = "wamv/base_link"
wrench_msg.wrench.force.x = wrench_body[0]
wrench_msg.wrench.force.y = wrench_body[1]
wrench_msg.wrench.torque.z = wrench_body[2]
self.wrench_pub.publish(wrench_msg)
# Publish reference pose for examination
self.pose_ref_pub.publish(PoseStamped(
header=Header(
frame_id='/enu',
stamp=msg.header.stamp,
),
pose=Pose(
position=Point(self.p_ref[0], self.p_ref[1], 0),
orientation=Quaternion(*self.q_ref),
),
))
# Publish adaptation (Y*theta) for plotting
adaptation_msg = WrenchStamped()
adaptation_msg.header.frame_id = "wamv/base_link"
adaptation_msg.wrench.force.x = (self.dist_est + self.drag_effort)[0]
adaptation_msg.wrench.force.y = (self.dist_est + self.drag_effort)[1]
adaptation_msg.wrench.torque.z = (self.dist_est + self.drag_effort)[2]
self.adaptation_pub.publish(adaptation_msg)
try:
self.theta_pub.publish(Float64MultiArray(data=self.drag_est))
except BaseException:
traceback.print_exc()
import numpy
import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped
from std_msgs.msg import Float32, String, Bool
from nav_msgs.msg import Odometry
freq = 20.0 # hz
dt = 1 / freq
error = numpy.zeros(3)
last_error = numpy.zeros(3)
error_sum = numpy.zeros(3)
velocity = numpy.zeros(3)
vel = TwistStamped()
localPose = PoseStamped()
targetAcquired = False
landingFlag = False
kp = rospy.get_param("/vel_control_kp")
kd = rospy.get_param("/vel_control_kd")
ki = rospy.get_param("/vel_control_ki")
altitudeSetpoint = rospy.get_param("/altitude_setpoint")
descentRate = rospy.get_param("/descent_rate")
# noinspection PyStatementEffect
def move(msg):
"""
var1, var2, var3 = raw_input("Enter target position").split()
sub_temp1 = rospy.Subscriber("/gazebo/iris_1/temperature", Float64,
tuto.temperature_1)
sub_temp2 = rospy.Subscriber("/gazebo/iris_2/temperature", Float64,
tuto.temperature_2)
sub_temp2 = rospy.Subscriber("/gazebo/iris_3/temperature", Float64,
tuto.temperature_3)
#sub_dist1=rospy.Subscriber("/gazebo/iris_1/fire_dist", Float64 , tuto.dist_1)
#sub_dist2=rospy.Subscriber("/gazebo/iris_2/fire_dist", Float64 , tuto.dist_2)
#sub_dist3=rospy.Subscriber("/gazebo/iris_3/fire_dist", Float64 , tuto.dist_3)
while True:
var = 0
vres = numpy.array([0.0, 0.0])
#iris_1
goal = PoseStamped()
x_off = 0.0
y_off = 0.0
separation = tuto.separation("iris_1")
cohesion = tuto.cohesion("iris_1")
v2 = numpy.array([(float(var1) + x_off), float(var2) + y_off])
tuto.vres[0] = separation[0] + (0.3 * cohesion[0])
tuto.vres[1] = separation[1] + (0.3 * cohesion[1])
goal.header.frame_id = "/base_link"
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = tuto.vres[0] + x_off
goal.pose.position.y = tuto.vres[1] + y_off
goal.pose.position.z = float(var3)
tuto.arm_pub[0].publish(goal)
tuto.vres = numpy.array([0.0, 0.0])
#iris_2
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") ]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") ]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral") ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 1
rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling')))
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) )
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) )
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.4
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.07, 0.007, 0.10]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.36
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = (table_ground + table_size[2] / 2.0)-0.08
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
box1_pose.pose.position.y = 0.0
box1_pose.pose.position.z = (table_ground + table_size[2] + box1_size[2] / 2.0)-0.08
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
box2_pose.pose.position.y = 0.2
box2_pose.pose.position.z = (table_ground + table_size[2] + box2_size[2] / 2.0)-0.08
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = (table_ground + table_size[2] + target_size[2] / 2.0)-0.075
target_pose.pose.orientation.w = 0.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = table_pose.pose.position.x - 0.15
place_pose.pose.position.y = -0.23
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 0.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] + self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def create_statemachine(self):
try:
response = self.relative_work_point_srv()
except rospy.ServiceException as e:
rospy.logerr('servive call failed:{}'.format(e))
relative_grasp_pose = PoseStamped()
relative_grasp_pose2 = PoseStamped()
relative_grasp_pose3 = PoseStamped()
relative_grasp_pose4 = PoseStamped()
relative_grasp_pose5 = PoseStamped()
relative_grasp_pose6 = PoseStamped()
relative_assemble_pose = PoseStamped()
relative_assemble_pose2 = PoseStamped()
relative_assemble_pose3 = PoseStamped()
relative_assemble_pose4 = PoseStamped()
relative_assemble_pose5 = PoseStamped()
relative_assemble_pose6 = PoseStamped()
relative_grasp_pose.pose = response.grasp[0]
relative_grasp_pose2.pose = response.grasp[1]
relative_grasp_pose3.pose = response.grasp[2]
relative_grasp_pose4.pose = response.grasp[3]
relative_grasp_pose5.pose = response.grasp[4]
relative_grasp_pose6.pose = response.grasp[5]
relative_assemble_pose.pose = response.assemble[0]
relative_assemble_pose2.pose = response.assemble[1]
relative_assemble_pose3.pose = response.assemble[2]
relative_assemble_pose4.pose = response.assemble[3]
relative_assemble_pose5.pose = response.assemble[4]
relative_assemble_pose6.pose = response.assemble[5]
sm_top = StateMachine(outcomes=['success', 'failed'])
with sm_top:
StateMachine.add(
"InitialState",
InitialState(relative_grasp_pose),
transitions={
'success': 'SUB1',
'failed': 'failed'})
sm_sub1 = StateMachine(outcomes=['success', 'failed'])
with sm_sub1:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose2),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB1", sm_sub1, transitions={
'success': 'SUB2', 'failed': 'failed'})
sm_sub2 = StateMachine(outcomes=['success', 'failed'])
with sm_sub2:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose2),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose2),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose3),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB2", sm_sub2, transitions={
'success': 'SUB3', 'failed': 'failed'})
sm_sub3 = StateMachine(outcomes=['success', 'failed'])
with sm_sub3:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose3),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose3),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose4),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB3", sm_sub3, transitions={
'success': 'SUB4', 'failed': 'failed'})
sm_sub4 = StateMachine(outcomes=['success', 'failed'])
with sm_sub4:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose4),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose4),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose5),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB4", sm_sub4, transitions={
'success': 'SUB5', 'failed': 'failed'})
sm_sub5 = StateMachine(outcomes=['success', 'failed'])
with sm_sub5:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose5),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose5),
transitions={
'success': 'AfterAssembleRepetition',
'failed': 'failed'})
StateMachine.add(
"AfterAssembleRepetition",
AfterAssembleRepetition(relative_grasp_pose6),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB5", sm_sub5, transitions={
'success': 'SUB6', 'failed': 'failed'})
sm_sub6 = StateMachine(outcomes=['success', 'failed'])
with sm_sub6:
StateMachine.add(
"BeforeGrasp",
BeforeGrasp(),
transitions={
'success': 'AfterGrasp',
'failed': 'failed'})
StateMachine.add(
"AfterGrasp",
AfterGrasp(relative_assemble_pose6),
transitions={
'success': 'BeforeAssemble',
'failed': 'failed'})
StateMachine.add(
"BeforeAssemble",
BeforeAssemble(relative_assemble_pose6),
transitions={
'success': 'AfterAssemble',
'failed': 'failed'})
StateMachine.add(
"AfterAssemble",
AfterAssemble(),
transitions={
'success': 'success',
'failed': 'failed'})
StateMachine.add(
"SUB6",
sm_sub6,
transitions={
'success': 'FinalState',
'failed': 'failed'})
StateMachine.add(
"FinalState",
FinalState(),
transitions={
'success': 'success',
'failed': 'failed'})
sis = IntrospectionServer('p_and_p_example', sm_top, '/SM_ROOT')
sis.start()
sm_top.execute()
rospy.spin()
class Tutorial:
arm_pub = list()
arm_pub.append(
rospy.Publisher("gazebo/iris_1/go_to_destination",
PoseStamped,
queue_size=1000))
arm_pub.append(
rospy.Publisher("gazebo/iris_2/go_to_destination",
PoseStamped,
queue_size=1000))
arm_pub.append(
rospy.Publisher("gazebo/iris_3/go_to_destination",
PoseStamped,
queue_size=1000))
coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
max = 0
previous_temp_1 = 0
previous_temp_2 = 0
previous_temp_3 = 0
previous_x_1 = 0
previous_y_1 = 0
previous_x_2 = 0
previous_y_2 = 0
previous_x_3 = 0
previous_y_3 = 0
vres = numpy.array([0.0, 0.0])
further_pose = PoseStamped()
_blockListDict = {
'block_a': Block('iris_1', 'base_link'),
'block_b': Block('iris_2', 'base_link'),
'block_c': Block('iris_3', 'base_link')
}
def separation(self, uav_name):
model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
input_drone_coordinates = model_coordinates(uav_name, "base_link")
c_x = input_drone_coordinates.pose.position.x
c_y = input_drone_coordinates.pose.position.y
a = random.random()
f = random.uniform(-0.005, 0.005)
#return vector
v = numpy.array([0.0, 0.0])
#add random quantity
#position=numpy.array([c_x+f,c_y+f])
position = numpy.array([c_x, c_y])
#radius
radius = 3
n_count = 0
for block in self._blockListDict.itervalues():
blockName = str(block._name)
if blockName != uav_name:
resp_coordinates = model_coordinates(
blockName, block._relative_entity_name)
x1 = resp_coordinates.pose.position.x
x2 = input_drone_coordinates.pose.position.x
y1 = resp_coordinates.pose.position.y
y2 = input_drone_coordinates.pose.position.y
distance = math.sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))
#Finding neibourghs
if distance < radius:
v[0] += x1 - x2
v[1] += y1 - y2
n_count = n_count + 1
#print "v after n count"
#print v
#Divide by the number of neibourgh
if n_count == 0:
print "I am moving randomly..."
return position
v[0] = v[0] / n_count
v[1] = v[1] / n_count
#calculate magnitude
magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
print "magnitude separation"
print magnitude
v = v / magnitude
print "unitary vector"
print v
#print "v after n count division"
#print v
#Opposite direction
v[0] *= -2
v[1] *= -2
print "negated vector"
print v
#print "v opposite direction"
#print v
#Normalize vector using min-max normalization
#print "v after normalization"
#print v
return v
#def alignment(self, uav_name):
def cohesion(self, uav_name):
model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
input_drone_coordinates = model_coordinates(uav_name, "base_link")
c_x = input_drone_coordinates.pose.position.x
c_y = input_drone_coordinates.pose.position.y
a = random.random()
f = random.uniform(-0.005, 0.005)
print "input drone"
print uav_name
print c_x
print c_y
#return vector
v = numpy.array([0.0, 0.0])
#add random quantity
position = numpy.array([c_x + f, c_y + f])
#radius
radius = 50
n_count = 0
for block in self._blockListDict.itervalues():
blockName = str(block._name)
if blockName != uav_name:
resp_coordinates = model_coordinates(
blockName, block._relative_entity_name)
x1 = resp_coordinates.pose.position.x
x2 = input_drone_coordinates.pose.position.x
y1 = resp_coordinates.pose.position.y
y2 = input_drone_coordinates.pose.position.y
distance = math.sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))
#Finding neibourghs
if distance < radius:
v[0] += x1
v[1] += y1
n_count = n_count + 1
#print "v after n count"
#print v
#Divide by the number of neibourgh
if n_count == 0:
return position
#return center of mass
v[0] = v[0] / n_count
v[1] = v[1] / n_count
#print "v after n count division"
#print v
#calculate directio for center of mass
v[0] = v[0] - input_drone_coordinates.pose.position.x
v[1] = v[1] - input_drone_coordinates.pose.position.y
#Normalize vector using min-max normalization
magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
v = v / magnitude
#print "v after normalization"
#print v
return v
def temperature_1(self, temperature):
print "I am in temperature"
print temperature
goal = PoseStamped()
x_off = 0.0
y_off = 0.0
separation = tuto.separation("iris_1")
cohesion = tuto.cohesion("iris_1")
model_coordinates_1 = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model_coordinates = model_coordinates_1("iris_1", "base_link")
v = numpy.array([0.0, 0.0])
if temperature > 60:
if self.previous_temp_1 < temperature:
current_x = model_coordinates.pose.position.x
current_y = model_coordinates.pose.position.y
v[0] = current_x - self.previous_x_1
v[1] = current_y - self.previous_y_1
magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
v = (v + 0.8) * (-1)
goal.header.frame_id = "/base_link"
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
0.3 * cohesion[0]) + x_off
goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
0.3 * cohesion[1]) + y_off
goal.pose.position.z = 1
self.arm_pub[0].publish(goal)
rospy.sleep(0.5)
self.previous_temp_1 = temperature
self.previous_x_1 = model_coordinates.pose.position.x
self.previous_y_1 = model_coordinates.pose.position.y
def temperature_2(self, temperature):
print "I am in temperature"
print temperature
goal = PoseStamped()
x_off = -2.0
y_off = -2.0
separation = tuto.separation("iris_2")
cohesion = tuto.cohesion("iris_2")
model_coordinates_2 = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model_coordinates = model_coordinates_2("iris_2", "base_link")
v = numpy.array([0.0, 0.0])
if temperature > 60:
if self.previous_temp_2 < temperature:
current_x = model_coordinates.pose.position.x
current_y = model_coordinates.pose.position.y
v[0] = current_x - self.previous_x_2
v[1] = current_y - self.previous_y_2
magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
v = (v + 0.8) * (-1)
print "vector"
print v
goal.header.frame_id = "/base_link"
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
0.3 * cohesion[0]) + x_off
goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
0.3 * cohesion[1]) + y_off
goal.pose.position.z = 1
self.arm_pub[1].publish(goal)
rospy.sleep(0.5)
self.previous_temp_2 = temperature
self.previous_x_2 = model_coordinates.pose.position.x
self.previous_y_2 = model_coordinates.pose.position.y
def temperature_3(self, temperature):
print "I am in temperature"
print temperature
goal = PoseStamped()
x_off = -2.0
y_off = 2.0
separation = tuto.separation("iris_3")
cohesion = tuto.cohesion("iris_3")
model_coordinates_3 = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
model_coordinates = model_coordinates_3("iris_3", "base_link")
v = numpy.array([0.0, 0.0])
if temperature > 60:
if self.previous_temp_3 < temperature:
current_x = model_coordinates.pose.position.x
current_y = model_coordinates.pose.position.y
v[0] = current_x - self.previous_x_3
v[1] = current_y - self.previous_y_3
magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
v = (v + 0.8) * (-1)
print "vector"
print v
goal.header.frame_id = "/base_link"
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
0.3 * cohesion[0]) + x_off
goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
0.3 * cohesion[1]) + y_off
goal.pose.position.z = 1
self.arm_pub[2].publish(goal)
rospy.sleep(0.5)
self.previous_temp_3 = temperature
self.previous_x_3 = model_coordinates.pose.position.x
self.previous_y_3 = model_coordinates.pose.position.y
def measurement_callback(msg):
# rospy.logwarn(msg)
for marker_detect in msg.markers:
rospy.logwarn(marker_detect)
marker = PoseStamped()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "aruco/detected" + str(marker_detect.id)
marker.pose = marker_detect.pose.pose
# rospy.loginfo(marker.header.frame_id)
if not tf_buf.can_transform(marker.header.frame_id, 'map',
marker.header.stamp, rospy.Duration(0.5)):
rospy.logwarn_throttle(
5.0, 'No transform from %s to map' % marker.header.frame_id)
return
aruco_frame = 'aruco/marker' + str(marker_detect.id)
detect_look = tf_buf.lookup_transform('cf1/odom',
marker.header.frame_id,
marker.header.stamp)
aruco_look = tf_buf.lookup_transform('map', aruco_frame,
marker.header.stamp)
detect_look.transform.rotation.w = -detect_look.transform.rotation.w
q_diff = quaternion_multiply(aruco_look.transform.rotation,
detect_look.transform.rotation)
rospy.sleep(0.1)
rot_trans(detect_look, q_diff[1], q_diff[2], q_diff[3], q_diff[0])
rot_marker = TransformStamped()
rot_marker.header.stamp = rospy.Time.now()
rot_marker.header.frame_id = 'odomRot'
rot_marker.child_frame_id = 'rot_marker' + str(marker_detect.id)
rot_marker.transform.translation = detect_look.transform.translation
rot_marker.transform.rotation.w = 1
br.sendTransform(rot_marker)
rospy.sleep(0.1)
rot_marker_frame = 'rot_marker' + str(marker_detect.id)
detect_look_new = tf_buf.lookup_transform('cf1/odom', rot_marker_frame,
marker.header.stamp)
odom = TransformStamped()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'map'
odom.child_frame_id = 'cf1/odom'
odom.transform.rotation.x = q_diff[1]
odom.transform.rotation.y = q_diff[2]
odom.transform.rotation.z = q_diff[3]
odom.transform.rotation.w = q_diff[0]
odom.transform.translation.x = (
aruco_look.transform.translation.x -
detect_look_new.transform.translation.x)
odom.transform.translation.y = (
aruco_look.transform.translation.y -
detect_look_new.transform.translation.y)
odom.transform.translation.z = (
aruco_look.transform.translation.z -
detect_look_new.transform.translation.z)
br2.sendTransform(odom)
def trajectory_cb(self, msg):
if len(msg.points) == 0:
print("ToppTracker - empty input trajectory recieved, RESET")
self.trajectory = MultiDOFJointTrajectory()
return
if not self.carrot_trajectory_recieved:
print("ToppTracker - trajectory recieved but carrot unavailable")
self.trajectory = MultiDOFJointTrajectory()
return
# Nicely interpolate points from current to first
x, y, z, yaw = self.interpolate_points(self.carrot_trajectory,
msg.points[0])
print(x)
print(y)
print(yaw)
if len(x) == 0:
x.append(self.carrot_trajectory.transforms[0].translation.x)
y.append(self.carrot_trajectory.transforms[0].translation.y)
z.append(self.carrot_trajectory.transforms[0].translation.z)
yaw.append(
tf.transformations.euler_from_quaternion([
self.carrot_trajectory.transforms[0].rotation.x,
self.carrot_trajectory.transforms[0].rotation.y,
self.carrot_trajectory.transforms[0].rotation.z,
self.carrot_trajectory.transforms[0].rotation.w
])[2])
for point in msg.points:
x.append(point.transforms[0].translation.x)
y.append(point.transforms[0].translation.y)
z.append(point.transforms[0].translation.z)
yaw.append(
tf.transformations.euler_from_quaternion([
point.transforms[0].rotation.x,
point.transforms[0].rotation.y,
point.transforms[0].rotation.z,
point.transforms[0].rotation.w
])[2])
# Fix Toppra orientation, at this point atleast two points are in trajectory
if len(yaw) > 1:
yaw[-1] = self.fix_topp_yaw(yaw[-1], yaw[-2])
for x_, y_, z_, yaw_ in zip(x, y, z, yaw):
print("Recieved point: ", x_, y_, z_, yaw_)
request = GenerateTrajectoryRequest()
# Add waypoints in request
waypoint = JointTrajectoryPoint()
for i in range(0, len(x)):
# Positions are defined above
waypoint.positions = [x[i], y[i], z[i], yaw[i]]
# Also add constraints for velocity and acceleration. These
# constraints are added only on the first waypoint since the
# TOPP-RA reads them only from there.
if i == 0:
waypoint.velocities = [
self.tracker_params.velocity[0],
self.tracker_params.velocity[1],
self.tracker_params.velocity[2],
self.tracker_params.velocity[3]
]
waypoint.accelerations = [
self.tracker_params.acceleration[0],
self.tracker_params.acceleration[1],
self.tracker_params.acceleration[2],
self.tracker_params.acceleration[3]
]
# Append all waypoints in request
request.waypoints.points.append(copy.deepcopy(waypoint))
request.waypoints.joint_names = ["x", "y", "z", "yaw"]
request.sampling_frequency = self.tracker_params.sampling_frequency
request.n_gridpoints = self.tracker_params.n_gridpoints
request.plot = False
# Request the trajectory
request_trajectory_service = rospy.ServiceProxy(
"generate_toppra_trajectory", GenerateTrajectory)
response = request_trajectory_service(request)
# Response will have trajectory and bool variable success. If for some
# reason the trajectory was not able to be planned or the configuration
# was incomplete or wrong it will return False.
print("ToppTracker: Converting trajectory to multi dof")
joint_trajectory = response.trajectory
self.enable_trajectory = False
# Take the first point in the message, extract its roll / pitch and copy it in the
# final trajectory
angles = tf.transformations.euler_from_quaternion([
msg.points[0].transforms[0].rotation.x,
msg.points[0].transforms[0].rotation.y,
msg.points[0].transforms[0].rotation.z,
msg.points[0].transforms[0].rotation.w
])
self.trajectory = self.JointTrajectory2MultiDofTrajectory(
joint_trajectory, angles[0], angles[1])
# Publish the path
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = msg.header.frame_id
self.trajectory_pose_arr.header.stamp = rospy.Time.now()
self.trajectory_pose_arr.header.frame_id = msg.header.frame_id
self.trajectory_pose_arr.poses = []
self.trajectory_index = 0
for i, point in enumerate(self.trajectory.points):
path_point = PoseStamped()
path_point.header.stamp = rospy.Time.now()
path_point.header.frame_id = msg.header.frame_id
path_point.pose.position.x = point.transforms[0].translation.x
path_point.pose.position.y = point.transforms[0].translation.y
path_point.pose.position.z = point.transforms[0].translation.z
path_point.pose.orientation.x = point.transforms[0].rotation.x
path_point.pose.orientation.y = point.transforms[0].rotation.y
path_point.pose.orientation.z = point.transforms[0].rotation.z
path_point.pose.orientation.w = point.transforms[0].rotation.w
path_msg.poses.append(path_point)
if i % 10 != 0:
continue
self.trajectory_pose_arr.poses.append(path_point.pose)
self.path_pub.publish(path_msg)