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