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 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
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
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()