History

PotentialField component

This is the PotentialField component, originally from the GenoM3 OSMOSIS show case, adapte to take also use a PointCloud.

Using a Pontential Field method. When the robot is located in the port Pose (in) current position, it produces a speed Cmd in port PFCmd (out) (linear and angular), trying to reach the port Target (in) position. It is thus attracted by its goal and pushed away from laser hit (obstacles) found in the port Scan (in) laser scan. The implementation is strongly inspired from (and the matlab code provided by the authors). @article{Guerra:2016kv, author = {Guerra, M and Efimov, D and Zheng, G and Perruquetti, W}, title = {{Avoiding local minima in the potential field method using input-to-state stability}}, journal = {Control Engineering Practice}, year = {2016}, volume = {55}, number = {C}, pages = {174—184}, month = oct, publisher = {Elsevier}, doi = {10.1016/j.conengprac.2016.07.008}, }

This implementation only consider one "composite" obstacle…

Ports

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 obstacles.

Scan (in)

Data structure

struct ::fidl::sensor::sensor_msgs_LaserScan Scan
- struct ::fidl::std::std_msgs_Header header
  - unsigned long seq
  - struct ::fidl::std::std_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 obstacles.

Target (in)

Data structure

struct ::or::t3d::pos Target
- double x
- double y
- double z

The position to reach for now.

Pose (in)

Data structure

struct ::or_pose_estimator::state Pose
- 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::pos_cov > pos_cov
  - double cov[28]
- 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::vel_cov > vel_cov
  - double cov[21]
- 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::pos_cov > pos_cov
  - double cov[6]
- optional< struct ::or::t3d::att_cov > att_cov
  - double cov[10]
- optional< struct ::or::t3d::att_pos_cov > att_pos_cov
  - double cov[12]
- optional< struct ::or::t3d::vel_cov > vel_cov
  - double cov[6]
- optional< struct ::or::t3d::avel_cov > avel_cov
  - double cov[6]
- optional< struct ::or::t3d::acc_cov > acc_cov
  - double cov[6]
- optional< struct ::or::t3d::aacc_cov > aacc_cov
  - double cov[6]

The current position of the robot.

PFCmd (out)

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 as the result (linear.x and angular.z).

PF_Att (out)

Data structure

struct ::or::rb3d::force PF_Att
- double x
- double y
- double z

Potential Field Attraction Force.

PF_Rep (out)

Data structure

struct ::or::rb3d::force PF_Rep
- double x
- double y
- double z

Potential Field Repulsion Force.

PF_V (out)

Data structure

struct ::or::rb3d::force PF_V
- double x
- double y
- double z

Potential Field V Force.

PF_Res (out)

Data structure

struct ::or::rb3d::force PF_Res
- double x
- double y
- double z

Potential Field Resulting Force.

OccupancyGrid (out)

Data structure

struct ::fidl::nav::nav_msgs_OccupancyGrid OccupancyGrid
- struct ::fidl::std::std_msgs_Header header
  - unsigned long seq
  - struct ::fidl::std::std_msgs_time stamp
    
    unsigned long sec
    
    unsigned long nsec
  - string frame_id
- struct ::fidl::nav::nav_msgs_MapMetaData info
  - struct ::fidl::std::std_msgs_time map_load_time
    
    unsigned long sec
    
    unsigned long nsec
  - float resolution
  - unsigned long width
  - unsigned long height
  - struct ::fidl::nav::geometry_msgs_Pose origin
    
    struct ::fidl::nav::geometry_msgs_Point position
    
    double x
    
    double y
    
    double z
    
    struct ::fidl::nav::geometry_msgs_Quaternion orientation
    
    double x
    
    double y
    
    double z
    
    double w
- sequence< octet > data

To visualise the map in rviz.

Services

GetIDS (attribute)

Outputs

struct ::PotentialField::ids ids
- double hit_max_distance
- double hit_min_distance
- double hit_min_z
- double hit_max_z
- double repForce_distance
- double nu
- double psi
- double epsilon
- double alpharep
- double goal_tolerance
- 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
- boolean new_map
- unsigned long obstacles[200][100]
- struct ::PotentialField::ids::cmd_s cmd
  - double vx
  - double wz
- struct ::PotentialField::ids::cmd_s max_speed
  - double vx
  - double wz
- struct ::PotentialField::ids::pose_s pose
  - double x
  - double y
  - double theta
- struct ::PotentialField::ids::pose_s target
  - double x
  - double y
  - double theta

Get the whole IDS (mostly useful for debuging).

SetDistanceToGoal (attribute)

Inputs

double goal_tolerance (default "0.8")

Set the distance at which we consider we have reached the goal.

SetHitMaxDistance (attribute)

Inputs

double hit_max_distance (default "5")

Set the maximum distance at which laser hits are considered

SetHitMinDistance (attribute)

Inputs

double hit_min_distance (default "0.5")

Set the minimum distance at which laser hits are considered

SetHitMinZ (attribute)

Inputs

double hit_min_z (default "-1")

Set the minimum z at which we consider hit (w.r.t. the sensor frame).

SetHitMaxZ (attribute)

Inputs

double hit_max_z (default "1")

Set the maximum z at which we consider hit (w.r.t. the sensor frame).

SetRepForceDistance (attribute)

Inputs

double repForce_distance (default "2")

Set the distance at which hits generate repulsive force

SetNu (attribute)

Inputs

double nu (default "3")

Throws

exception ::PotentialField::bad_value

Set the Nu potential field algo parameter.

SetPsi (attribute)

Inputs

double psi (default "6")

Throws

exception ::PotentialField::bad_value

Set Psi potential field algo parameter.

SetAlpha (attribute)

Inputs

double alpharep (default "0.2")

Set Alpha potential field algo parameter.

SetEpsilon (attribute)

Inputs

double epsilon (default "0.11")

Set Epsilon potential field algo parameter.

SetMaxSpeed (attribute)

Inputs

struct ::PotentialField::ids::cmd_s max_speed
- double vx (default "1")
- double wz (default "0.3")

Throws

exception ::PotentialField::bad_value

Set the max speed values (vx and wz).

SetRobotRadius (attribute)

Inputs

double robot_radius (default "0.6")

Set the robot_radius parameter.

Stop (function)

Context

Interrupts StartTrackTargetPort (activity)
Interrupts WriteSpeedInPort (activity)

Stop the tracking of the target port.

StopPublishOccupancyGrid (function)

Context

Interrupts PublishOccupancyGrid (activity)

Stop the publishing of the OccupancyGrid port.

SetVerbose (function)

Inputs

long verbose_level (default "0") Verbose level

Set the verbose level.

PrintObstaclesMap (function)

Print the map of obstacles gathered from the scans.

Init (activity)

Context

In task plan (frequency 20.0 Hz)
Updates port PFCmd (out)

Initialise by first making sure we are stopped.

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 ::PotentialField::bad_speed_cmd_port

Context

In task plan (frequency 20.0 Hz)
Updates port PFCmd (out)
Interrupts StartTrackTargetPort (activity)
Interrupts WriteSpeedInPort (activity)

Write in the port PFCmd (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.

StartTrackTargetPort (activity)

Throws

exception ::PotentialField::bad_speed_cmd_port
exception ::PotentialField::bad_scan_port
exception ::PotentialField::bad_target_port
exception ::PotentialField::bad_pose_port
exception ::PotentialField::out_of_mem
exception ::PotentialField::bad_force_port

Context

In task plan (frequency 20.0 Hz)
Reads port PCL (in)
Reads port Scan (in)
Reads port Target (in)
Reads port Pose (in)
Updates port PFCmd (out)
Updates port PF_Att (out)
Updates port PF_Rep (out)
Updates port PF_V (out)
Updates port PF_Res (out)
Interrupts StartTrackTargetPort (activity)
Interrupts WriteSpeedInPort (activity)

Produce the port PFCmd (out) speed command to go and stay at the port Target (in), avoiding ostacles in the Scan (in) port. This service does not terminate (unless stopped by the Stop (function) service) and will servo control to stay at the Target (in) position.

PublishOccupancyGrid (activity)

Throws

exception ::PotentialField::bad_og_port

Context

In task plan (frequency 20.0 Hz)
Reads port Pose (in)
Updates port OccupancyGrid (out)
Interrupts PublishOccupancyGrid (activity)

Output the OccupancyGrid port.

Tasks

plan

Context

Frequency 20.0 Hz
Updates port PFCmd (out)
Updates port OccupancyGrid (out)

Project

General

Profile

François Felix Ingrand » OSMOSIS » PotentialField-genom3

Indexes

PotentialField component

Ports

PCL (in)

Scan (in)

Target (in)

Pose (in)

PFCmd (out)

PF_Att (out)

PF_Rep (out)

PF_V (out)

PF_Res (out)

OccupancyGrid (out)

Services

GetIDS (attribute)

SetDistanceToGoal (attribute)

SetHitMaxDistance (attribute)

SetHitMinDistance (attribute)

SetHitMinZ (attribute)

SetHitMaxZ (attribute)

SetRepForceDistance (attribute)

SetNu (attribute)

SetPsi (attribute)

SetAlpha (attribute)

SetEpsilon (attribute)

SetMaxSpeed (attribute)

SetRobotRadius (attribute)

Stop (function)

StopPublishOccupancyGrid (function)

SetVerbose (function)

PrintObstaclesMap (function)

Init (activity)

WriteSpeedInPort (activity)

StartTrackTargetPort (activity)

PublishOccupancyGrid (activity)

Tasks

plan