def land_mach_client():
client = actionlib.SimpleActionClient('land_machine_action',DoLandMachineAction)
client.wait_for_server()
fis_names = ['x_fis','y_fis','z_fis','T_fis']
fis_dicts = [rospy.get_param(name) for name in fis_names]
fis_list = [fisyaml.fis_from_dict(d) for d in fis_dicts]
fis_array = [fisyaml.fis_to_ros_msg(f) for f in fis_list]
goal = DoLandMachineGoal()
goal.fis_array.fis_array = fis_array
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
def land_machine_action_client(csc):
"""
assume that we are only giving it an xy fIS and a z FIS for now.
"""
rospy.loginfo('land_machine_action_client: received individual')
client = actionlib.SimpleActionClient('land_machine_action',irols.msg.DoLandMachineAction)
client.wait_for_server()
rospy.loginfo('land_machine_action_client: client is live')
fis_array = [fisyaml.fis_to_ros_msg(fis) for fis in csc.gfs]
fis_array = [fis_array[0]]*2 + [fis_array[1]]*2
goal = irols.msg.DoLandMachineGoal()
goal.fis_array.fis_array = fis_array
client.send_goal(goal)
rospy.loginfo('land_machine_action_client: goal is sent')
client.wait_for_result()
result = client.get_result()
return result.fitness
def execute(self,goal):
rospy.loginfo('{0}: received goal: {1}'.format(self._action_name,goal))
self.ga.genMax = goal.num_gen
self.ga.popSize = goal.pop_size
self._feedback.curr_min_cost = float('inf')
for gen in xrange(goal.num_gen-1):
self._feedback.curr_gen = gen
self._as.publish_feedback(self._feedback)
self.eval_population()
self.ga.iter_generation()
self._feedback.curr_gen += 1
self.eval_population()
self._result.min_cost = self.ga.fitness_hist[-1]
fis_array = [fisyaml.fis_to_ros_msg(fis) for fis in self.ga.pop_curr[0].gfs]
fis_array = [fis_array[0]]*2 + [fis_array[1]]*2
self._result.fis_array.fis_array = fis_array
self._as.set_succeeded(self._result)
def eval_population(self):
"""
Reach into GA and do things manually for the sake of reporting
"""
fitnessvalues = []
for i,ind in enumerate(self.ga.pop_curr):
if self._as.is_preempt_requested():
self._result.min_cost = self._feedback.curr_min_cost
fis_array = [fisyaml.fis_to_ros_msg(fis) for fis in self.ga.pop_curr[self.cur_best_ind].gfs]
fis_array = [fis_array[0]]*2 + [fis_array[1]]*2
self._result.fis_array.fis_array = fis_array
fitnessvalues.append((ind,self.ga.fitness(ind)))
if fitnessvalues[-1][-1]<self._feedback.curr_min_cost:
self._feedback.curr_min_cost = fitnessvalues[-1][-1]
self.cur_best_ind = i
self._feedback.curr_cost = fitnessvalues[-1][-1]
self._feedback.curr_ind = i
self._as.publish_feedback(self._feedback)
sorted_fitness = zip(*sorted(fitnessvalues,key=itemgetter(1)))
self.ga.pop_curr = sorted_fitness[0]
self.ga.fitness_hist.append(sorted_fitness[1][0])
def eval_population(self):
"""
Reach into GA and do things manually for the sake of reporting
"""
fitnessvalues = []
for i,ind in enumerate(self.ga.pop_curr):
if len(self.bests) > 0:
rospy.loginfo('{0}: (cur_elite == cur_best)={1}'.format(self._action_name,self.bests[-1]==self.ga.pop_curr[0]))
if self._as.is_preempt_requested():
self._result.min_cost = self._feedback.curr_min_cost
fis_array = [fisyaml.fis_to_ros_msg(fis) for fis in self.ga.pop_curr[self.cur_best_ind].gfs]
fis_array = [fis_array[0]]*2 + [fis_array[1]]*2
self._result.fis_array.fis_array = fis_array
fitnessvalues.append((ind,self.ga.fitness(ind)))
if fitnessvalues[-1][-1]<self._feedback.curr_min_cost:
self._feedback.curr_min_cost = fitnessvalues[-1][-1]
self.cur_best_ind = i
self._feedback.curr_cost = fitnessvalues[-1][-1]
self._feedback.curr_ind = i
self._as.publish_feedback(self._feedback)
sorted_fitness = zip(*sorted(fitnessvalues,key=itemgetter(1)))
self.ga.pop_curr = sorted_fitness[0]
self.ga.fitness_hist.append(sorted_fitness[1][0])
def execute(self,goal):
rospy.loginfo('{0}: received goal: {1}'.format(self._action_name,goal))
self.cur_best_ind = 0
self.bests = []
if goal.num_gen:
self.ga.genMax = goal.num_gen
if goal.pop_size:
self.ga.popSize = goal.pop_size
self.ga.init_population()
self.ga.seed_pop(self.csc)
self._feedback.curr_min_cost = float('inf')
for gen in xrange(self.ga.genMax-1):
self._feedback.curr_gen = gen
self.eval_population()
self.bests.append(self.ga.pop_curr[0])
self.ga.iter_generation()
self._feedback.curr_gen += 1
self.eval_population()
self._result.min_cost = self.ga.fitness_hist[-1]
fis_array = [fisyaml.fis_to_ros_msg(fis) for fis in self.ga.pop_curr[0].gfs]
fis_array = [fis_array[0]]*2 + [fis_array[1]]*2
self._result.fis_array.fis_array = fis_array
self._as.set_succeeded(self._result)