Esempio n. 1
0
    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()
Esempio n. 2
0
    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()
Esempio n. 3
0
    def relative_move(self, x=0, y=0, z=0, tx=0, ty=0, tz=0, timeout=10):
        '''
            Move to a position relative to the current position
            The user should specify position with
            class_instance.relative_goal and then call this function
        '''

        to_send = Waypoint()

        # Add move onto current position
        to_send.pose.position.x = self.current_pos.pose.position.x + x
        to_send.pose.position.y = self.current_pos.pose.position.y + y
        to_send.pose.position.z = self.current_pos.pose.position.z + z

        # This section adds the desired axis rotation to current position
        # convert from quaternion to numpy array
        cur_rotation = gh.quat_to_euler(gh.quat_to_np(self.current_pos.pose.orientation))
        new_rotation = cur_rotation
        new_rotation[0] = cur_rotation[0] + tx
        new_rotation[1] = cur_rotation[1] + ty
        new_rotation[2] = cur_rotation[2] + tz
        # convert from rotvec back into ROS quaternion message
        quat = gh.euler_to_quat(new_rotation)
        # set quaternion to send as the new quaternion
        to_send.pose.orientation = mh.numpy_to_quat(quat)
        # send goal to server to complete within timeout
        move_result = self.send_goal(to_send, timeout)
        return move_result
Esempio n. 4
0
    def relative_move(self, x=0, y=0, z=0, tx=0, ty=0, tz=0, timeout=10):
        '''
            Move to a position relative to the current position
            The user should specify position with
            class_instance.relative_goal and then call this function
        '''

        to_send = Waypoint()

        # Add move onto current position
        to_send.pose.position.x = self.current_pos.pose.position.x + x
        to_send.pose.position.y = self.current_pos.pose.position.y + y
        to_send.pose.position.z = self.current_pos.pose.position.z + z

        # This section adds the desired axis rotation to current position
        # convert from quaternion to numpy array
        cur_rotation = gh.quat_to_euler(
            gh.quat_to_np(self.current_pos.pose.orientation))
        new_rotation = cur_rotation
        new_rotation[0] = cur_rotation[0] + tx
        new_rotation[1] = cur_rotation[1] + ty
        new_rotation[2] = cur_rotation[2] + tz
        # convert from rotvec back into ROS quaternion message
        quat = gh.euler_to_quat(new_rotation)
        # set quaternion to send as the new quaternion
        to_send.pose.orientation = mh.numpy_to_quat(quat)
        # send goal to server to complete within timeout
        move_result = self.send_goal(to_send, timeout)
        return move_result
Esempio n. 5
0
 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()
Esempio n. 6
0
 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()