Project

General

Profile

History

SafetyPilot component

This is the SafetyPilot component from the GenoM3 OSMOSIS show case. It can also be used with minnie by using a PointCloud instead of Laserscan.

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

  • port Joystick (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 Joystick (in), we do not check Scan (in) so it can be used to tak control back and move the robot away from an obstacle.

Ports

Cmd (out)

Data structure

  • struct ::or_rigid_body::state Cmd

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::jerk > jerk

      • double jx

      • double jy

      • double jz

    • optional< struct ::or::t3d::snap > snap

      • double sx

      • double sy

      • double sz

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 to check for the closest obstacle.

PCL (in)

Data structure

  • struct ::or::pcl PCL

    • struct ::or::time::ts timeStamp

      • long sec

      • long nsec

    • unsigned short msgVersion

    • string sensorId

    • string frameId

    • boolean isRegistered

    • boolean isOrdered

    • unsigned long height

    • unsigned long width

    • boolean hasFixedTransform

    • struct ::or::transform pose_robotFrame_sensorFrame

      • struct ::or::t3d::pos translation

        • double x

        • double y

        • double z

      • struct ::or::t3d::att orientation

        • double qw

        • double qx

        • double qy

        • double qz

      • struct ::or::t3d::pos_cov tcov

        • double cov[6]

      • struct ::or::t3d::att_cov ocov

        • double cov[10]

      • struct ::or::t3d::att_pos_cov tocov

        • double cov[12]

    • struct ::or::transform pose_fixedFrame_robotFrame

      • struct ::or::t3d::pos translation

        • double x

        • double y

        • double z

      • struct ::or::t3d::att orientation

        • double qw

        • double qx

        • double qy

        • double qz

      • struct ::or::t3d::pos_cov tcov

        • double cov[6]

      • struct ::or::t3d::att_cov ocov

        • double cov[10]

      • struct ::or::t3d::att_pos_cov tocov

        • double cov[12]

    • sequence< struct ::or::t3d::pos > points

      • double x

      • double y

      • double z

    • sequence< struct ::or::color > colors

      • double r

      • double v

      • double b

    • sequence< short > intensity

The pcl to check for the closest obstacle.

PFCmd (in)

Data structure

  • struct ::or_rigid_body::state PFCmd

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::jerk > jerk

      • double jx

      • double jy

      • double jz

    • optional< struct ::or::t3d::snap > snap

      • double sx

      • double sy

      • double sz

The speed command produced by the Potential Field component.

Joystick (in)

Data structure

  • struct ::or_joystick::state Joystick

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • sequence< boolean, 16 > buttons

    • sequence< short, 16 > axes

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

GetIDS (attribute)

Outputs

  • struct ::SafetyPilot::ids ids

    • double hit_min_distance

    • double stop_distance

    • double stop_lateral_distance

    • long min_pcl_hit

    • long min_hit

    • struct ::SafetyPilot::ids::cmd_s max_speed

      • double vx

      • double wz

    • boolean teleop_ok

    • 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

    • struct ::or_rigid_body::state IDSPFCmd

      • struct ::or::time::ts ts

        • long sec

        • long nsec

      • boolean intrinsic

      • optional< struct ::or::t3d::pos > pos

        • double x

        • double y

        • double z

      • optional< struct ::or::t3d::att > att

        • double qw

        • double qx

        • double qy

        • double qz

      • optional< struct ::or::t3d::vel > vel

        • double vx

        • double vy

        • double vz

      • optional< struct ::or::t3d::avel > avel

        • double wx

        • double wy

        • double wz

      • optional< struct ::or::t3d::acc > acc

        • double ax

        • double ay

        • double az

      • optional< struct ::or::t3d::aacc > aacc

        • double awx

        • double awy

        • double awz

      • optional< struct ::or::t3d::jerk > jerk

        • double jx

        • double jy

        • double jz

      • optional< struct ::or::t3d::snap > snap

        • double sx

        • double sy

        • double sz

    • struct ::or_rigid_body::state IDSCmd

      • struct ::or::time::ts ts

        • long sec

        • long nsec

      • boolean intrinsic

      • optional< struct ::or::t3d::pos > pos

        • double x

        • double y

        • double z

      • optional< struct ::or::t3d::att > att

        • double qw

        • double qx

        • double qy

        • double qz

      • optional< struct ::or::t3d::vel > vel

        • double vx

        • double vy

        • double vz

      • optional< struct ::or::t3d::avel > avel

        • double wx

        • double wy

        • double wz

      • optional< struct ::or::t3d::acc > acc

        • double ax

        • double ay

        • double az

      • optional< struct ::or::t3d::aacc > aacc

        • double awx

        • double awy

        • double awz

      • optional< struct ::or::t3d::jerk > jerk

        • double jx

        • double jy

        • double jz

      • optional< struct ::or::t3d::snap > snap

        • double sx

        • double sy

        • double sz

Get the whole IDS (mostly useful for debuging).

SetHitMinDistance (attribute)

Inputs

  • double hit_min_distance (default "0.4")

Set the minimum distance at which laser hits are considered

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.

SetMaxSpeed (attribute)

Inputs

  • struct ::SafetyPilot::ids::cmd_s max_speed

    • double vx (default "1")

    • double wz (default "0.3")
Throws

  • exception ::SafetyPilot::bad_value

Set the max speed values (vx and wz).

SetMinHit (attribute)

Inputs

  • long min_hit (default "500")

Set the max_angular speed.

SetMinGenHit (attribute)

Inputs

  • long min_gen_hit (default "3")

Stop (function)

Context

Stop SpeedMergeAndStopIfObstacle (activity).

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_joystick_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 Joystick (in) is active (somebody uses the JoyStick), it skips the check_obstable codel. Otherwise, it get a fresh port Scan (in) or PCL (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).

JoystickOn (activity)

Throws

  • exception ::SafetyPilot::bad_speed_cmd_port

  • exception ::SafetyPilot::bad_joystick_port

  • exception ::SafetyPilot::out_of_mem
Context

This activity starts pure Joystick control. It first checks the availability of the port. Then it loops until it is stopped, by grabbing the speed from the Joystick only. It then writes the speed in port Cmd (out). The stop codel sets and writes a null linear and angular speed in Cmd (out).

WriteSpeedInPort (activity)

Inputs

  • double vx (default "0") Linear speed along x axis in m/s.

  • double wz (default "0") Angular velocity around z in rad/s.
Throws

  • exception ::SafetyPilot::bad_speed_cmd_port
Context

Write in the port Cmd (out) the speed passed as argument. This is a debugging activity, use at your own risk. This service does not terminate (unless stopped by the Stop (function) service) and will just loop writting the same speed.

Tasks

pilot

Context

Pilot task, at 50 ms.