def tx_pose(self): '''Slighty safer to use.''' if self.test_mode: yield self.nh.sleep(.1) blank = mil_ros_tools.pose_to_numpy(Odometry().pose.pose) defer.returnValue(blank) next_odom_msg = yield self._odom_sub.get_next_message() pose = mil_ros_tools.pose_to_numpy(next_odom_msg.pose.pose) defer.returnValue(pose)
def tx_pose(self): '''Slighty safer to use.''' if self.test_mode: yield self.nh.sleep(.1) blank = mil_ros_tools.pose_to_numpy(Odometry().pose.pose) defer.returnValue(blank) next_odom_msg = yield self._odom_sub.get_next_message() pose = mil_ros_tools.pose_to_numpy(next_odom_msg.pose.pose) defer.returnValue(pose)
def publish_odom(self, *args): if self.last_odom is None or self.position_offset is None: return msg = self.last_odom if self.target in msg.name: header = mil_ros_tools.make_header(frame='/map') target_index = msg.name.index(self.target) twist = msg.twist[target_index] # Add position offset to make the start position (0, 0, -depth) position_np, orientation_np = mil_ros_tools.pose_to_numpy( msg.pose[target_index]) pose = mil_ros_tools.numpy_quat_pair_to_pose( position_np - self.position_offset, orientation_np) self.state_pub.publish(header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) header = mil_ros_tools.make_header(frame='/world') twist = msg.twist[target_index] pose = msg.pose[target_index] self.world_state_pub.publish( header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) else: # fail return
def sanity_check(sub, est_pos): yield None est_pos_np = pose_to_numpy(est_pos.pose)[0] print(est_pos_np == np.array([5, 5, 5])).all() if (est_pos_np == np.array([5, 5, 5])).all(): print "BUOY MISSION - Problem with guess." defer.returnValue(None) if est_pos_np[2] > -0.2: print 'BUOY MISSION - Detected buoy above the water' defer.returnValue(None) defer.returnValue(True)
def state_cb(self, msg): ''' Position is offset so first message is taken as zero point. (More reflective of actual sub). Z position is absolute and so is rotation. TODO: Add noise ''' if self.target not in msg.name: return if (self.last_odom is None or self.position_offset is None): pose = msg.pose[msg.name.index(self.target)] position, orientation = mil_ros_tools.pose_to_numpy(pose) position[2] = 0.0 self.position_offset = position self.last_odom = msg
def publish_odom(self, *args): if self.last_odom is None or self.position_offset is None: return msg = self.last_odom if self.target in msg.name: header = mil_ros_tools.make_header(frame='/map') target_index = msg.name.index(self.target) twist = msg.twist[target_index] # Add position offset to make the start position (0, 0, -depth) position_np, orientation_np = mil_ros_tools.pose_to_numpy(msg.pose[ target_index]) pose = mil_ros_tools.numpy_quat_pair_to_pose( position_np - self.position_offset, orientation_np) self.state_pub.publish( header=header, child_frame_id='/base_link', pose=PoseWithCovariance( pose=pose ), twist=TwistWithCovariance( twist=twist ) ) header = mil_ros_tools.make_header(frame='/world') twist = msg.twist[target_index] pose = msg.pose[target_index] self.world_state_pub.publish( header=header, child_frame_id='/base_link', pose=PoseWithCovariance( pose=pose ), twist=TwistWithCovariance( twist=twist ) ) else: # fail return
def set_odom(msg): return setattr(self, "odom", pose_to_numpy(msg.pose.pose))
def set_odom(msg): return setattr(self, "odom", pose_to_numpy(msg.pose.pose))