def odom_callback(self, current_pos):
self.lock.acquire()
self.odom_active = True
# Grab current position and orientation and 0 linear Z and angluar X and Y
# Get current position
current_position = mh.ros_to_np_3D(current_pos.pose.pose.position)
current_orientation = gh.quat_to_euler(current_pos.pose.pose.orientation)
# Zero unneccesary elements
current_position[2] = 0
current_orientation[0:2] = 0
# Add current position to state i_history
self.current_state = numpy.concatenate([current_position, current_orientation])
# Get current velocities
current_lin_vel = mh.ros_to_np_3D(current_pos.twist.twist.linear)
current_ang_vel = mh.ros_to_np_3D(current_pos.twist.twist.angular)
# Add current velocities to velocity i_history
self.current_velocity = numpy.concatenate([current_lin_vel, current_ang_vel])
# If the desired state has not yet been set, set desired and current as the same
# Resets the controller to current position on bootup
if (not self.desired_state_set):
self.desired_state = self.current_state
self.desired_state_set = True
self.lock.release()
self.main_loop()
def odom_callback(self, current_pos):
self.lock.acquire()
self.odom_active = True
# Grab current position and orientation and 0 linear Z and angluar X and Y
# Get current position
current_position = mh.ros_to_np_3D(current_pos.pose.pose.position)
current_orientation = gh.quat_to_euler(
current_pos.pose.pose.orientation)
# Zero unneccesary elements
current_position[2] = 0
current_orientation[0:2] = 0
# Add current position to state i_history
self.current_state = numpy.concatenate(
[current_position, current_orientation])
# Get current velocities
current_lin_vel = mh.ros_to_np_3D(current_pos.twist.twist.linear)
current_ang_vel = mh.ros_to_np_3D(current_pos.twist.twist.angular)
# Add current velocities to velocity i_history
self.current_velocity = numpy.concatenate(
[current_lin_vel, current_ang_vel])
# If the desired state has not yet been set, set desired and current as the same
# Resets the controller to current position on bootup
if (not self.desired_state_set):
self.desired_state = self.current_state
self.desired_state_set = True
self.lock.release()
self.main_loop()
def trajectory_callback(self, desired_trajectory):
self.lock.acquire()
self.desired_state_set = True
# Get desired pose and orientation
desired_pose = mh.ros_to_np_3D(desired_trajectory.posetwist.pose.position)
desired_orientation = gh.quat_to_euler(desired_trajectory.posetwist.orientation)
# Get desired linear and angular velocities
desired_lin_vel = mh.ros_to_np_3D(desired_trajectory.posetwist.twist.linear)
desired_ang_vel = mh.ros_to_np_3D(desired_trajectory.posetwist.twist.angular)
# Add desired position to desired state i_historys
self.desired_state = numpy.concatenate([desired_pose, desired_orientation])
# Add desired velocities to velocity i_history
self.desired_velocity = numpy.concatenate([desired_lin_vel, desired_ang_vel])
self.lock.release()
def trajectory_callback(self, desired_trajectory):
self.lock.acquire()
self.desired_state_set = True
# Get desired pose and orientation
desired_pose = mh.ros_to_np_3D(
desired_trajectory.posetwist.pose.position)
desired_orientation = gh.quat_to_euler(
desired_trajectory.posetwist.orientation)
# Get desired linear and angular velocities
desired_lin_vel = mh.ros_to_np_3D(
desired_trajectory.posetwist.twist.linear)
desired_ang_vel = mh.ros_to_np_3D(
desired_trajectory.posetwist.twist.angular)
# Add desired position to desired state i_historys
self.desired_state = numpy.concatenate(
[desired_pose, desired_orientation])
# Add desired velocities to velocity i_history
self.desired_velocity = numpy.concatenate(
[desired_lin_vel, desired_ang_vel])
self.lock.release()
