Project

General

Profile

SafetyPilot component

This is the SafetyPilot component from the GenoM3 OSMOSIS show case.

It produces a speed Cmd in port Cmd (out) (linear and angular) from:

  • port TeleopCmd (in) if present and active, or

  • port PFCmd (in) (PotentialField) if present and there are no obstacle, found in the port Scan (in) laser scan,

  • a null speed if a too close obstacle is found in Scan (in).

Note Note that when we use TeleopCmd (in), we do not check Scan (in) so it can be used to move the robot away from an obstacle.

Ports

Cmd (out)

Data structure
  • struct ::geometry::geometry_msgs_Twist Cmd

    • struct ::geometry::geometry_msgs_Vector3 linear

      • double x

      • double y

      • double z

    • struct ::geometry::geometry_msgs_Vector3 angular

      • double x

      • double y

      • double z

The speed command produced (linear.x and angular.z).


Scan (in)

Data structure
  • struct ::laser::sensor_msgs_LaserScan Scan

    • struct ::laser::std_msgs_header header

      • unsigned long seq

      • struct ::laser::msgs_time stamp

        • unsigned long sec

        • unsigned long nsec

      • string frame_id

    • float angle_min

    • float angle_max

    • float angle_increment

    • float time_increment

    • float scan_time

    • float range_min

    • float range_max

    • sequence< float > ranges

    • sequence< float > intensities

The laser scan to check for the closest obstacle.


PFCmd (in)

Data structure
  • struct ::geometry::UnicycleVelocity PFCmd

    • double linear

    • double angular

The speed command produced by the Potential Field component.


TeleopCmd (in)

Data structure
  • struct ::Teleop::TeleopUnicycleVelocity TeleopCmd

    • struct ::geometry::UnicycleVelocity velocity

      • double linear

      • double angular

    • boolean active

The speed command produced by Teleop. It is the same as above with an additional active flag to figure out if it is active or not.


Services

SetStopDistance (attribute)

Inputs
  • double stop_distance (default "0.8")

Set the stop_distance.


SetStopLateralDistance (attribute)

Inputs
  • double stop_lateral_distance (default "0.3")

Set the stop_lateral_distance.


SetMaxLinear (attribute)

Inputs
  • double max_linear (default "1")

Set the max_linear speed.


SetMaxAngular (attribute)

Inputs
  • double max_angular (default "0.4")

Set the max_angular speed.


SetVerbose (function)

Inputs
  • long verbose_level (default "0") Verbose level

Set the verbose level.


SpeedMergeAndStopIfObstacle (activity)

Throws
  • exception ::SafetyPilot::bad_scan_port

  • exception ::SafetyPilot::bad_speed_cmd_port

  • exception ::SafetyPilot::bad_pf_cmd_port

  • exception ::SafetyPilot::bad_teleop_cmd_port

  • exception ::SafetyPilot::out_of_mem

Context

This activity starts the SafetyPilot. It first checks the availability of the ports. Then it loops until it is stopped. If the port TeleopCmd (in) is active (somebody uses the JoyStick), it skips the check_obstable codel. Otherwise, it get a fresh port Scan (in), then checks for obstacle and if none are found passes the PFCmd (in). It then writes the speed in port Cmd (out). The stop codel sets and writes a null linear and angular speed in Cmd (out).


Tasks

pilot

Context
  • Frequency 25.0 Hz

Pilot task, at 40 ms.