def loop(self):
# distance away from point in NE
t = rospy.Time.now().to_sec() % self.circle_period
added_circle = np.array([self.circle_radius * np.cos(2*np.pi * t / self.circle_period),
self.circle_radius * np.sin(2*np.pi * t / self.circle_period)])
if self.point_source == SOURCE_NAV_REAL:
if self.point_ll is None:
return
self.ne = fm.geo2ne(self.point_ll, self.circler_origin, self.geo_radius)
elif self.point_source == SOURCE_NAV_SIM:
if self.ne is None:
return
# elif self.point_source == SOURCE_STATIC:
# pass
req_position = np.zeros(6)
req_position[0:2] = self.ne + added_circle
pr = PilotRequest()
pr.header.stamp = rospy.Time.now()
pr.position = req_position
self.pos_req_pub.publish(pr)
def loop(self):
if self.switch is False:
return
if self.points is None:
return
if self.index >= len(self.points):
rospy.loginfo('%s: Path complete!', self.name)
self.clear_path()
return
if rospy.Time.now().to_sec() - self.t_start > self.timeout:
rospy.logwarn('%s: Timeout! Clearing the path!', self.name)
self.clear_path()
self.send_zero_req()
return
des_point = self.points[self.index]
if self.point_reached(des_point):
rospy.loginfo('%s: Point reached: %s.', self.name, des_point[0:2])
self.index += 1
else:
pr = PilotRequest()
pr.header.stamp = rospy.Time.now()
pr.position = des_point
self.position_pub.publish(pr)
def parse_body_req(self, payload_type, id, dispatch_time, body, origin_address):
values = struct.unpack(FORMAT[payload_type], body)
pilot_msg = PilotRequest()
pilot_msg.header.stamp = rospy.Time.from_sec(dispatch_time)
pilot_msg.position = list(values[0:6])
self.pub_body.publish(pilot_msg)
def _do_hover_callback(self, req):
goal = req.goal.values
rospy.loginfo(
"DoHover: received goal [{0}, {1}, {2}, {4}, {5}]".format(
goal[0], goal[1], goal[2], goal[3], goal[4], goal[5]))
action_start_time = rospy.get_time()
timeoutReached = False
# take the received goal and push it to the motor controls semantics
# get a publisher
pub = rospy.Publisher("/pilot/position_req", PilotRequest)
goalNED = NED()
goalNED.north, goalNED.east, goalNED.depth = goal[0], goal[1], goal[2]
goalRPY = RPY()
goalRPY.roll, goalRPY.pitch, goalRPY.yaw = goal[3], goal[4], goal[5]
# repeatedly call world waypoint req to move the robot to the goal
# while the robot is not at teh desired location
while not (rospy.is_shutdown()
or self._nav != None and utils.epsilonEqualsNED(
self._nav.position, goalNED, 0.5, depth_e=0.6)
and utils.epsilonEqualsY(self._nav.orientation, goalRPY, .2)
# we compare on just pitch and yaw for 5 axis robot
):
# publish the goal repeatedly
pilotMsg = PilotRequest()
pilotMsg.position = list(goal)
pub.publish(pilotMsg)
# rospy.loginfo("Sent Goal!!")
rospy.sleep(0.5)
if timeoutReached:
return DoHoverResponse(False)
print("Sleeping for a while ...")
rospy.sleep(4)
# pub and subscriber will be removed at the end of executes context
rospy.loginfo("DoHover: complete")
return DoHoverResponse(True)
rospy.sleep(2)
logNav(log_nav=True, dir_path="/tmp/", file_name_descriptor=str(0))
for wp in wps:
wp_msg = PilotRequest()
wp_msg.disable_axis = [0, 1, 0, 0, 0, 0]
goal_msg = BehavioursGoal()
cnt = 0
while not rospy.is_shutdown():
if wp != wps[0]:
angle = math.atan2(wp[1] - _stats.position.east,
wp[0] - _stats.position.north)
# new_heading = environment.raw_angle_to_goal + _stats.orientation.yaw
wp_msg.position = [wp[0], wp[1], 0, 0, 0, angle]
else:
wp_msg.position = [wp[0], wp[1], 0, 0, 0, wp[2]]
goal_msg.goal_position = Vector6(
values=[wp[0], wp[1], 0, 0, 0, wp[2]])
pub_wp.publish(wp_msg)
pub_behaviour_goal.publish(goal_msg)
rospy.sleep(PUBLISH_RATE)
print("cnt: {0}\ndistance: {1}".format(
cnt, environment.distance_to_goal))
if abs(_stats.orientation.yaw - wp[2]) < 0.1:
def pilotPublishPositionRequest(self, pose):
request = PilotRequest()
request.position = pose
self.pilot_position_request.publish(request)
def send_zero_req(self):
br = PilotRequest()
br.header.stamp = rospy.Time.now()
br.position = np.zeros(6).tolist()
rospy.loginfo('%s: Sending stay request at: %s', self.name, self.pose[0:2])
self.body_pub.publish(br)