Python posetwist_to_numpyの例

コード例 #1

ファイル: base_controller.py プロジェクト: zachgoins/Sub8

    def trajectory_cb(self, msg):
        '''
        Message structure:
            - header: frame, timestamp
            - waypoint[]:
                - pose
                - twist
        '''
        print msg
        self.state_history = {
            'position': deque(),
            'linear_vel': deque(),
            'orientation_q': deque(),
            'angular_vel': deque(),
        }
        rospy.logwarn("Recieving trajectory!")
        self.target_trajectory = {
            'position': deque(),
            'linear_vel': deque(),
            'orientation_q': deque(),
            'angular_vel': deque(),
        }
        for struct_waypoint in msg.trajectory:
            # Deserialize to numpy
            pose, twist = sub8_utils.posetwist_to_numpy(struct_waypoint)
            (position, orientation_q), (linear_vel, angular_vel) = pose, twist

            # TODO: Less repeated code
            self.target_trajectory['position'].append(position)
            self.target_trajectory['linear_vel'].append(linear_vel)
            self.target_trajectory['orientation_q'].append(orientation_q)
            self.target_trajectory['angular_vel'].append(angular_vel)

コード例 #2

ファイル: base_controller.py プロジェクト: ErolB/Sub8

    def trajectory_cb(self, msg):
        '''
        Message structure:
            - header: frame, timestamp
            - waypoint[]:
                - pose
                - twist
        '''
        print msg
        self.state_history = {
            'position': deque(),
            'linear_vel': deque(),
            'orientation_q': deque(),
            'angular_vel': deque(),
        }
        rospy.logwarn("Recieving trajectory!")
        self.target_trajectory = {
            'position': deque(),
            'linear_vel': deque(),
            'orientation_q': deque(),
            'angular_vel': deque(),
        }
        for struct_waypoint in msg.trajectory:
            # Deserialize to numpy
            pose, twist = sub8_utils.posetwist_to_numpy(struct_waypoint)
            (position, orientation_q), (linear_vel, angular_vel) = pose, twist

            # TODO: Less repeated code
            self.target_trajectory['position'].append(position)
            self.target_trajectory['linear_vel'].append(linear_vel)
            self.target_trajectory['orientation_q'].append(orientation_q)
            self.target_trajectory['angular_vel'].append(angular_vel)

コード例 #3

ファイル: controller_verify.py プロジェクト: zachgoins/Sub8

 def traj_cb(self, msg):
     pose, twist = sub8_utils.posetwist_to_numpy(msg.trajectory[0])
     self.target_state = pose[0]