def __init__(self):
self.waypoint_client = actionlib.SimpleActionClient('waypoint', waypointAction)
self.waypoint_client.wait_for_server()
self.waypointGoal = waypointGoal()
self.position = Point()
rospy.Subscriber("base_pose_ground_truth", Odometry, callback=self.drone_position_callback)
rospy.wait_for_message("base_pose_ground_truth", Odometry)
xlims = rospy.get_param("xlims","[0,20]")
ylims = rospy.get_param("ylims","[0,20]")
Range = namedtuple("Range", "min max")
xlims = loads(xlims)
ylims = loads(ylims)
self.xbounds = Range(xlims[0], xlims[1])
self.ybounds = Range(ylims[0], ylims[1])
self.map_boundary_reached = False
self.has_reached_waypoint = True
self._waypoint_resolution = 0.5
self._position_epsilon = 1e-4
self.waypoint_prev = self.get_drone_position()
self.waypoint = self.get_drone_position()
self.waypoint_heading = self.drone_heading
self.wayPoint = Point()
self.wayPoint.x = self.wayPoint.y = None
self.wayPoint_prev = self.position
def sendWaypoint(self, x, y, z):
# Send waypoint and set has_reached_waypoint to false. Set this back to true when the feedback from server comes true
self.has_reached_waypoint = False
self.waypointGoal = waypointGoal([Point(x, y, z)])
self.waypoint_client.send_goal(self.waypointGoal,
feedback_cb=self.actionFeedback,
done_cb=self.actionDone)
def sendWaypoint(self, waypoint, wait_for_result=True):
# Send waypoint and set has_reached_waypoint to false. Set this back to true when the feedback from server comes true
self.has_reached_waypoint = False
self.waypointGoal = waypointGoal([Point(waypoint[0], waypoint[1], waypoint[2])])
self.waypoint_client.send_goal(self.waypointGoal, done_cb=self.actionDone)
if wait_for_result:
self.waypoint_client.wait_for_result()
self.waypoint_prev = self.get_drone_position()
def __init__(self):
self.waypoint_client = actionlib.SimpleActionClient('waypoint', waypointAction)
self.waypoint_client.wait_for_server()
self.waypointGoal = waypointGoal()
self.drone_position = Point()
rospy.Subscriber("base_pose_ground_truth", Odometry, callback=self.drone_position_callback)
rospy.wait_for_message("base_pose_ground_truth", Odometry)
self.has_reached_waypoint = True
self._waypoint_resolution = 0.5
self._position_epsilon = 1e-4
self.waypoint_prev = self.drone_position
self.waypoint = self.drone_position
self.waypoint_heading = self.drone_heading
def __init__(self):
self.simulation_time = 0.0
self.drone_x = 0.0
self.drone_y = 0.0
self.drone_heading = 0.0
self.concentration_curr = 0.0
self.concentration_prev = 0.0
self.wind_speed = 0.0
self.wind_direction = 0.0
self.max_source_prob_x = 0.0
self.max_source_prob_y = 0.0
self.max_source_prob_z = 0.0
self._position_epsilon = 1e-4
# Define some epsilon based on the sensor configuration
# Random value. Change later
self._concentration_epsilon = 1e-4
# Define some lamba. Based on what?
# Random value. Change later
self._probability_threshold = 1e-4
self._move_step = 0.5
self.waypoint_client = actionlib.SimpleActionClient('waypoints', waypointAction)
self.waypoint_client.wait_for_server()
self.goal = waypointGoal()
self.waypoint_x = 0.0
self.waypoint_y = 0.0
self.waypoint_z = 3.0
self.waypoint_heading = self.getNewHeuristic()
self.has_reached_waypoint = True
def sendWayPoint(self):
self.has_reached_waypoint = False
self.waypointGoal = waypointGoal([self.wayPoint])
self.waypoint_client.send_goal(self.waypointGoal, done_cb=self.actionDone)
self.wayPoint_prev = self.wayPoint
def __init__(self):
# catch errors for simulation_time or initialise it to an appropriate value
self.simulation_time = 1.0
self.source_reached = False
# drone_spawn_heading is not yet added to rosparam
try:
self.drone_x = rospy.get_param(
"/crazyflie_pose_transform/drone_spawn_x")
self.drone_y = rospy.get_param(
"/crazyflie_pose_transform/drone_spawn_y")
self.drone_z = rospy.get_param(
"/crazyflie_pose_transform/drone_spawn_z")
self.drone_heading = rospy.get_param(
"/crazyflie_pose_transform/drone_spawn_yaw")
except KeyError:
raise rospy.ROSInitException
print(self.drone_x, self.drone_y, self.drone_heading)
self.concentration_curr = 0.0
self.concentration_prev = 0.0
self.wind_speed = 0.0
self.wind_direction = 0.0
# This first initial readings from the sensor should be ignored as
# they are very variant and the plume might not have reached the drone
# in certain conditions. Assign the max probability of source as the
# position of drone.
self.max_source_prob_x = self.drone_x
self.max_source_prob_y = self.drone_y
self.max_source_prob_z = self.drone_z
self._position_epsilon = 1e-4
# Define some epsilon based on the sensor configuration
# Random value. Change later
self._concentration_epsilon = 1e-1
# Define some lamba. Based on what?
# Random value. Change later
self._probability_threshold = 1e-4
self.has_reached_waypoint = True
self._move_step = 0.5
self.skipping_max_source_probability = True
self.skip_max_source_probability_msg = 5
self.skip_max_source_probability_msg_count = 0
self.waypoint_client = actionlib.SimpleActionClient(
'waypoint', waypointAction)
self.waypoint_client.wait_for_server()
self.waypointGoal = waypointGoal()
# Previous waypoint would be the place where the drone is currently at
self.waypoint_x_prev = self.drone_x
self.waypoint_y_prev = self.drone_y
self.waypoint_z_prev = self.drone_z
self.waypoint_heading_prev = self.getNewWaypointHeading()
self.waypoint_x = self.drone_x
self.waypoint_y = self.drone_y
self.waypoint_z = self.drone_z
self.waypoint_heading = self.getNewWaypointHeading()
def __init__(self):
# drone_spawn_heading is not yet added to rosparam
self.FOLLOW_WIND = -1
self.UPWIND = 0
self.ZIGZAG = 1
self.METAHEURISTIC = 2
self.drone = MoveDroneClient()
try:
self.algorithm = rospy.get_param("~Algorithm", 2)
except KeyError:
raise rospy.ROSInitException
if self.algorithm == self.UPWIND:
rospy.loginfo("Algorithm = RASTER SCAN AND UPWIND")
elif self.algorithm == self.ZIGZAG:
rospy.loginfo("Algorithm = ZIGZAG")
elif self.algorithm == self.METAHEURISTIC:
rospy.loginfo("Algorithm = METAHEURISTIC")
else:
rospy.logfatal("Invalid input for algorithm")
Range = namedtuple("Range", "min max")
# Waypoint resolution parameters
self.waypoint_res = 0.5
self.waypoint_heading = self.drone.heading
# The following are to change the waypoint_resolution based on the concentration
self.res_range = Range(1, 2.5)
self.conc_range = Range(10.0, 200.0)
self.calcWaypointSlopeIntercept()
self.waypoint_client = actionlib.SimpleActionClient(
'waypoint', waypointAction)
self.waypoint_client.wait_for_server()
self.waypointGoal = waypointGoal()
self.raster = actionlib.SimpleActionClient("rasterScan",
rasterScanAction)
self.raster.wait_for_server()
self.raster_goal = rasterScanGoal()
self.listener = tf.TransformListener()
self.fixed_frame = rospy.get_param("fixed_frame", "map")
self.anemo_frame = rospy.get_param("anemometer_frame",
"anemometer_framself.meta_stde")
# Temperature parameter
self.Temp = 14.00
self.delta_Temp = 0.5
self.meta_std = 0.2
# Zig-zag angle
self.alpha = math.radians(35)
self.source_reached = False
self._position_epsilon = 1e-4
# Define some epsilon based on the sensor configuration
# Random value. Change later
self._conc_grad_epsilon = 0
# Minimum concentration to detect
self._conc_epsilon = 10
# Define some lamba. Based on what?
# Random value. Change later
self._probability_threshold = 1e-4
self.maintain_dir_prob = 0.4
self.max_probability_message_received = False
self.has_reached_waypoint = True
self.moving_to_source = False
self.source_reached = False
self.lost_plume = False
self.lost_plume_max = 3
self.max_conc_at = Point()
self.max_conc_val = None
self.concentration_hist = []
self.lost_plume_counter = 0
self.raster_scan_complete = True
self.initial_scan_complete = False
self.wind_hist = deque([])
self.len_wind_hist = 15
self.lost_distance = 1
#! /usr/bin/env python
import rospy
import actionlib
from crazyflie_control.msg import waypointAction, waypointGoal, waypointResult, waypointFeedback
from geometry_msgs.msg import Point
def define_waypoints():
p = [Point(7, 3, 0)]
return p
def feedback_cb(feedback):
print('[Feedback] No. of points completed = %d/%d' %
(feedback.num_completed, feedback.total_points))
rospy.init_node('action_client')
client = actionlib.SimpleActionClient('waypoints', waypointAction)
client.wait_for_server()
goal = waypointGoal()
goal.points = define_waypoints()
client.send_goal(goal, feedback_cb=feedback_cb)
client.wait_for_result()
print("Completion message: " + client.get_result().succ_msg)