Execute one step of navigation.
(self, input_data, timestamp)
| 406 | return False |
| 407 | |
| 408 | def run_step(self, input_data, timestamp): |
| 409 | """ |
| 410 | Execute one step of navigation. |
| 411 | """ |
| 412 | self.vehicle_control_event.clear() |
| 413 | self.timestamp = timestamp |
| 414 | self.clock_publisher.publish(Clock(rospy.Time.from_sec(timestamp))) |
| 415 | |
| 416 | # check if stack is still running |
| 417 | if self.stack_process and self.stack_process.poll() is not None: |
| 418 | raise RuntimeError("Stack exited with: {} {}".format( |
| 419 | self.stack_process.returncode, self.stack_process.communicate()[0])) |
| 420 | |
| 421 | # publish global plan to ROS once |
| 422 | if self._global_plan_world_coord and not self.global_plan_published: |
| 423 | self.global_plan_published = True |
| 424 | self.publish_plan() |
| 425 | |
| 426 | new_data_available = False |
| 427 | |
| 428 | # publish data of all sensors |
| 429 | for key, val in input_data.items(): |
| 430 | new_data_available = True |
| 431 | sensor_type = self.id_to_sensor_type_map[key] |
| 432 | if sensor_type == 'sensor.camera.rgb': |
| 433 | self.publish_camera(key, val[1]) |
| 434 | elif sensor_type == 'sensor.lidar.ray_cast': |
| 435 | self.publish_lidar(key, val[1]) |
| 436 | elif sensor_type == 'sensor.other.gnss': |
| 437 | self.publish_gnss(key, val[1]) |
| 438 | elif sensor_type == 'sensor.can_bus': |
| 439 | self.publish_can(key, val[1]) |
| 440 | elif sensor_type == 'sensor.hd_map': |
| 441 | self.publish_hd_map(key, val[1]) |
| 442 | else: |
| 443 | raise TypeError("Invalid sensor type: {}".format(sensor_type)) |
| 444 | |
| 445 | if self.use_stepping_mode(): |
| 446 | if self.step_mode_possible and new_data_available: |
| 447 | self.vehicle_control_event.wait() |
| 448 | # if the stepping mode is not used or active, there is no need to wait here |
| 449 | |
| 450 | return self.current_control |
nothing calls this directly
no test coverage detected