def configure(self): print('configure controller') st = Settings() # timing st.sample_time = self._sample_time st.update_time = self._update_time # robots configuration = omg.RegularPolyhedron(0.5, self._n_robots, np.pi).vertices.T configurationT = omg.RegularPolyhedron(0.5, self._n_robots, np.pi/2.).vertices.T init_ctr = [-3.5, -1.] terminal_ctr = [3.5, 1.] st.fleet_config = [P3DXPose(pose=[c[0], c[1], 0.]) for c in configuration] st.init_pose = [P3DXPose(pose=[init_ctr[0]+c[0], init_ctr[1]+c[1], np.pi/2.]) for c in configuration] st.terminal_pose = [P3DXPose(pose=[terminal_ctr[0]+c[0], terminal_ctr[1]+c[1], 0.]) for c in configurationT] # environment st.room = Room(position=[0., 0.], shape=[10., 5.]) obstacles = [] obstacles.append(Obstacle(pose=[-2., -2.3, np.pi/2.], shape=[4., 0.1])) obstacles.append(Obstacle(pose=[0.5, -1.5, np.pi/2.], shape=[0.35])) st.obstacles = obstacles st.robobst = self._robobst # set motionplanner self._mp_configure_topic.publish(st) # init gazebo self.init_gazebo(st) self._settings = st
def fire_motionplanner(self, time, current_pose): # print 'firing!' trigger = Trigger() trigger.goal = [P3DXPose(self._goal)] trigger.state = [P3DXPose(current_pose)] trigger.current_time = time self._mp_trigger_topic.publish(trigger)
def fire_motionplanner(self, time, pose0): print('firing!') self._trigger.goal = self._goal self._trigger.state = [P3DXPose(pose0[k][:]) for k in range(self._n_robots)] self._trigger.obstacles = [Obstacle(pose=self._robobst_est_pose[k], velocity=self._robobst_est_velocity[k]) for k in range(len(self._robobst))] self._trigger.current_time = time self._mp_trigger_topic.publish(self._trigger)
def get_goal(self, goal) : self.goal = [0.0, 0.0, 0.0] self.goal[0] = goal.pose.position.x self.goal[1] = goal.pose.position.y qt = goal.pose.orientation r, p, y = tf.transformations.euler_from_quaternion([qt.x, qt.y, qt.z, qt.w]) # self.goal[2] = y rospy.loginfo(self.goal) self._mp_goal_topic.publish(P3DXPose(self.goal)) self.new_goal_set = True