(self,sim_pose_msg=None)
| 214 | return cloud_msg |
| 215 | |
| 216 | def CreateTFMessage(self,sim_pose_msg=None): |
| 217 | # if self.opt.publish_odom: |
| 218 | # self.msg_tf.transforms.append(TransformStamped()) |
| 219 | # self.msg_tf.transforms[-1].header.stamp = self.ros_time |
| 220 | # self.msg_tf.transforms[-1].header.frame_id = "/map" |
| 221 | # self.msg_tf.transforms[-1].child_frame_id = "/odom" |
| 222 | # self.msg_tf.transforms[-1].transform.translation.x = 0.000 |
| 223 | # self.msg_tf.transforms[-1].transform.translation.y = 0.000 |
| 224 | # self.msg_tf.transforms[-1].transform.translation.z = 0.000 |
| 225 | # self.msg_tf.transforms[-1].transform.rotation.x = 0.000 |
| 226 | # self.msg_tf.transforms[-1].transform.rotation.y = 0.000 |
| 227 | # self.msg_tf.transforms[-1].transform.rotation.z = 0.000 |
| 228 | # self.msg_tf.transforms[-1].transform.rotation.w = 1.000 |
| 229 | # self.msg_tf.transforms.append(TransformStamped()) |
| 230 | # self.msg_tf.transforms[-1].header.stamp = self.ros_time |
| 231 | # self.msg_tf.transforms[-1].header.frame_id = "/odom" |
| 232 | # self.msg_tf.transforms[-1].child_frame_id = "/camera_link" |
| 233 | # self.msg_tf.transforms[-1].transform.translation.x = sim_pose_msg.pose.position.x |
| 234 | # self.msg_tf.transforms[-1].transform.translation.y = sim_pose_msg.pose.position.y |
| 235 | # self.msg_tf.transforms[-1].transform.translation.z = sim_pose_msg.pose.position.z |
| 236 | # self.msg_tf.transforms[-1].transform.rotation.x = sim_pose_msg.pose.orientation.x |
| 237 | # self.msg_tf.transforms[-1].transform.rotation.y = sim_pose_msg.pose.orientation.y |
| 238 | # self.msg_tf.transforms[-1].transform.rotation.z = sim_pose_msg.pose.orientation.z |
| 239 | # self.msg_tf.transforms[-1].transform.rotation.w = sim_pose_msg.pose.orientation.w |
| 240 | |
| 241 | self.msg_tf.transforms.append(TransformStamped()) |
| 242 | self.msg_tf.transforms[-1].header.stamp = self.ros_time |
| 243 | self.msg_tf.transforms[-1].header.frame_id = "/camera_link" |
| 244 | self.msg_tf.transforms[-1].child_frame_id = "/camera_rgb_frame" |
| 245 | self.msg_tf.transforms[-1].transform.translation.x = 0.000 |
| 246 | self.msg_tf.transforms[-1].transform.translation.y = 0 |
| 247 | self.msg_tf.transforms[-1].transform.translation.z = 0.000 |
| 248 | self.msg_tf.transforms[-1].transform.rotation.x = 0.00 |
| 249 | self.msg_tf.transforms[-1].transform.rotation.y = 0.00 |
| 250 | self.msg_tf.transforms[-1].transform.rotation.z = 0.00 |
| 251 | self.msg_tf.transforms[-1].transform.rotation.w = 1.00 |
| 252 | |
| 253 | self.msg_tf.transforms.append(TransformStamped()) |
| 254 | self.msg_tf.transforms[-1].header.stamp = self.ros_time |
| 255 | self.msg_tf.transforms[-1].header.frame_id = "/camera_rgb_frame" |
| 256 | self.msg_tf.transforms[-1].child_frame_id = "/camera_rgb_optical_frame" |
| 257 | self.msg_tf.transforms[-1].transform.translation.x = 0.000 |
| 258 | self.msg_tf.transforms[-1].transform.translation.y = 0.000 |
| 259 | self.msg_tf.transforms[-1].transform.translation.z = 0.000 |
| 260 | |
| 261 | if self.pitch_degree =='-45': |
| 262 | self.msg_tf.transforms[-1].transform.rotation.x = 0.0 |
| 263 | self.msg_tf.transforms[-1].transform.rotation.y = -0.9238795325112867 |
| 264 | self.msg_tf.transforms[-1].transform.rotation.z = 0.3826834323650898 |
| 265 | self.msg_tf.transforms[-1].transform.rotation.w = 0.0 |
| 266 | elif self.pitch_degree == '0': |
| 267 | self.msg_tf.transforms[-1].transform.rotation.x = -0.500 |
| 268 | self.msg_tf.transforms[-1].transform.rotation.y = 0.500 |
| 269 | self.msg_tf.transforms[-1].transform.rotation.z = -0.500 |
| 270 | self.msg_tf.transforms[-1].transform.rotation.w = 0.500 |
| 271 | else: |
| 272 | raise ValueError |
| 273 |
no outgoing calls
no test coverage detected