(position, orientation)
| 28 | return stamped_pose |
| 29 | |
| 30 | def get_pose_msg(position, orientation): |
| 31 | pose_goal = geometry_msgs.msg.Pose() |
| 32 | pose_goal.orientation.x = orientation[0] |
| 33 | pose_goal.orientation.y = orientation[1] |
| 34 | pose_goal.orientation.z = orientation[2] |
| 35 | pose_goal.orientation.w = orientation[3] |
| 36 | |
| 37 | pose_goal.position.x = position [0] |
| 38 | pose_goal.position.y = position[1] |
| 39 | pose_goal.position.z = position[2] |
| 40 | return pose_goal |
| 41 | |
| 42 | |
| 43 | def get_cloud(pts:NDArray , classes_colors:NDArray , frame = "world"): |