diff --git a/app/robotPathPlannerExamples/scripts/robotPathPlannerExample2.xml b/app/robotPathPlannerExamples/scripts/robotPathPlannerExample2.xml index 00b05889..7b824370 100644 --- a/app/robotPathPlannerExamples/scripts/robotPathPlannerExample2.xml +++ b/app/robotPathPlannerExamples/scripts/robotPathPlannerExample2.xml @@ -12,7 +12,7 @@ fakeMobileBaseTest - --holonomic + --context fakeMobileBaseTest --holonomic console diff --git a/app/rosExamples/scripts/rosExample3.xml b/app/rosExamples/scripts/rosExample3.xml index c85aa7a6..4ee82b46 100644 --- a/app/rosExamples/scripts/rosExample3.xml +++ b/app/rosExamples/scripts/rosExample3.xml @@ -1,5 +1,5 @@ -ros EXAMPLE3 +ros EXAMPLE3 (Experimental)/name> diff --git a/src/baseControl/main.cpp b/src/baseControl/main.cpp index 7b10f2d1..af1063b2 100644 --- a/src/baseControl/main.cpp +++ b/src/baseControl/main.cpp @@ -41,37 +41,7 @@ * velocity_pid also controls the motors in velocity modes, but it also implements and external closed loop on the realized cartesian trajectory. * openloop_no_pid/openloop_pid are similar, but the individual motors are controlled by sending pwm references instead of velocity commands. * The module also performs the odometry computation and publishes it onto a yarp port/ROS topic. - * - * Parameters required by this module are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:--------------:|:--------------------:|:-------:|:--------:|:------------------:|:-----------: |:-----------------------------------------------------------------:|:-----:| - * | GENERAL | robot_type | string | - | - | Yes | The robot kinematic model. Can be one of the following: ikart_V1,ikart_V2,cer | | - * | GENERAL | control_mode | string | - | velocity_no_pid | Yes | It can be one of the following: velocity_no_pid, velocity_pid, openloop_no_pid, openloop_pid | | - * | GENERAL | max_linear_vel | double | m/s | 0.0 | Yes | Limiter for robot maximum linear velocities | - | - * | GENERAL | max_angular_vel | double | deg/s | 0.0 | Yes | Limiter for robot maximum angular velocities | - | - * | GENERAL | max_linear_acc | double | m/s^2 | 0.0 | Yes | Limiter for robot maximum linear accelerations | - | - * | GENERAL | max_angular_acc | double | deg/s^2 | 0.0 | Yes | Limiter for robot maximum angular accelerations | - | - * | GENERAL | ratio_limiter_enabled | int | - | 1 | Yes | Initial estimation of robot position | - | - * | GENERAL | input_limiter_enabled | int | - | 0 | Yes | Initial estimation of robot position | - | - * | GENERAL | use_ROS | bool | - | false | Yes | Globally enable/disable the ROS system: node, topics etc | - | - * | MOTORS | max_motor_pwm | double | - | - | Yes | Limiter for motor pwm (when control mode is openloop) | - | - * | MOTORS | max_motor_vel | double | deg/s | - | Yes | Limiter for motor velocity (when control mode is velocity) | - | - * | MOTORS | motors_filer_enabled | string | - | - | Yes | Enables a low-pass filter on motor controller to prevent jerky motion. Can be one of the following: 0,1,2,4,8 | - | - * | JOYSTICK | linear_vel_at_full_control | double | m/s | - | Yes | Robot linear velocity when the joypad stick is at its maximum | e.g. 0. | - * | JOYSTICK | angular_vel_at_full_control | double | deg/s | - | Yes | Robot angular velocity when the joypad stick is at its maximum | e.g. 30 | - * | XXX_VELOCITY_PID | kp | double | - | - | Yes | Proportional gain for PID control on user cartesian command | XXX can be one of the following: LINEAR, ANGULAR, HEADING | - * | XXX_VELOCITY_PID | ki | double | - | - | Yes | Integral gain for PID control on user cartesian command | - | - * | XXX_VELOCITY_PID | kd | double | - | - | Yes | Derivative gain for PID control on user cartesian command | - | - * | XXX_VELOCITY_PID | max | double | - | - | Yes | Limiter for PID control on user cartesian command | - | - * | XXX_VELOCITY_PID | min | double | - | - | Yes | Limiter for PID control on user cartesian command | - | - * | ROS_GENERAL | node_name | string | - | - | No | Name of the ROS node | e.g. baseControl | - * | ROS_FOOTPRINT | topic_name | string | - | - | No | Name of the topic used to publish the robot footprint | e.g. /footprint | - * | ROS_FOOTPRINT | footprint_frame | string | - | - | No | Frame to which the footprint is attached | e.g. mobile_base_link | - * | ROS_FOOTPRINT | footprint_diameter | double | m | - | No | Diameter of the robot footprint | e.g. 0.5 | - * | ROS_ODOMETRY | topic_name | string | - | - | No | Name of the topic used to publish odometry info | e.g. /odometry | - * | ROS_ODOMETRY | odom_frame | string | - | - | No | Name of the odometry frame | e.g. odom | - * | ROS_ODOMETRY | base_frame | string | - | - | No | Name of the base frame | e.g. mobile_base_body_link | - * | ROS_INPUT | topic_name | string | - | - | No | Name of the topic used to control the robot from ROS | e.g. /cmd_vel | + * A detailed description of configuration parameters available for the module is provided in the README.md file. */ using namespace std; diff --git a/src/robotGoto/README.md b/src/robotGoto/README.md index 20d9902d..cdb4377a 100644 --- a/src/robotGoto/README.md +++ b/src/robotGoto/README.md @@ -1,6 +1,7 @@ # robotGoto This module performs a point-to-point local navigation. It receives a goal from the user either via RPC command or via yarp iNavigation2D interface and it computes the Cartesian velocities to be sent to [**baseControl**](https://github.com/robotology/navigation/tree/master/src/baseControl) module. - + The module can be configured to stop the navigation if an obstacle is detected, or perform obstacle avoidance using an APF-based approach. + ## YARP Connections * **/robotGoto/rpc** standard rpc port * **/robotGoto/control:o** this port is used by the module to send cartesian velocities commands to baseControl module. diff --git a/src/robotGoto/main.cpp b/src/robotGoto/main.cpp index 945fc5c7..4d0189c6 100644 --- a/src/robotGoto/main.cpp +++ b/src/robotGoto/main.cpp @@ -20,48 +20,9 @@ * \section robotGoto * robotGoto is a local navigation module. It receives a position cartesian target, either absolute (respect to the map origin) or relative (respect to the robot frame) and computes * the cartesian velocity commands to be sent to baseControl module. - * Four control modes are available: velocity_no_pid sets the control modes of the motors to velocity and sends individual velocity references, computed by from user cartesian commands, to the joints. - * velocity_pid also controls the motors in velocity modes, but it also implements and external closed loop on the realized cartesian trajectory. - * openloop_no_pid/openloop_pid are similar, but the individual motors are controlled by sending pwm references instead of velocity commands. - * The module also performs the odometry computation and publishes it onto a yarp port/ROS topic. - * - * Parameters required by this module are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:----------------:|:--------------------:|:-------:|:--------:|:------------------:|:-----------: |:-----------------------------------------------------------------:|:-----:| - * | LOCALIZATION | robot_frame_id | string | - | - | Yes | The robot kinematic model. Can be one of the following: ikart_V1,ikart_V2,cer | | - * | LOCALIZATION | map_frame_id | string | - | - | Yes | It can be one of the following: velocity_no_pid, velocity_pid, openloop_no_pid, openloop_pid | | - * | LASER | laser_port | string | - | - | Yes | Limiter for robot maximum linear velocities | - | - * | ROBOT_GEOMETRY | robot_radius | double | m | 0.0 | Yes | Limiter for robot maximum angular velocities | - | - * | ROBOT_GEOMETRY | laser_pos_x | double | m | 0.0 | Yes | Limiter for robot maximum linear accelerations | - | - * | ROBOT_GEOMETRY | laser_pos_y | double | m | 0.0 | Yes | Limiter for robot maximum angular accelerations | - | - * | ROBOT_GEOMETRY | laser_pos_theta | double | deg | 0.0 | Yes | Initial estimation of robot position | - | - * | ROBOT_TRAJECTORY | robot_is_holonomic | int | 0/1 | 0 | Yes | Initial estimation of robot position | - | - * | ROBOT_TRAJECTORY | max_beta_angle | double | - | | Yes | Initial estimation of robot position | - | - * | ROBOT_TRAJECTORY | ang_speed_gain | double | - | | Yes | Globally enable/disable the ROS system: node, topics etc | - | - * | ROBOT_TRAJECTORY | lin_speed_gain | double | - | - | Yes | Limiter for motor pwm (when control mode is openloop) | - | - * | ROBOT_TRAJECTORY | max_lin_speed | double | m/s | - | Yes | Limiter for motor velocity (when control mode is velocity) | - | - * | ROBOT_TRAJECTORY | max_ang_speed | double | deg/s | - | Yes | Enables a low-pass filter on motor controller to prevent jerky motion. Can be one of the following: 0,1,2,4,8 | - | - * | ROBOT_TRAJECTORY | min_lin_speed | double | m/s | - | Yes | Robot linear velocity when the joypad stick is at its maximum | e.g. 0. | - * | ROBOT_TRAJECTORY | min_ang_speed | double | deg/s | - | Yes | Robot angular velocity when the joypad stick is at its maximum | e.g. 30 | - * | ROBOT_TRAJECTORY | goal_tolerance_lin | double | m | - | Yes | Proportional gain for PID control on user cartesian command | XXX can be one of the following: LINEAR, ANGULAR, HEADING | - * | ROBOT_TRAJECTORY | goal_tolerance_angle | double | deg | - | Yes | Integral gain for PID control on user cartesian command | - | - * | RETREAT_OPTION | enable_retreat | int | 0/1 | - | Yes | Derivative gain for PID control on user cartesian command | - | - * | RETREAT_OPTION | retreat_duration | double | s | - | Yes | Limiter for PID control on user cartesian command | - | - * | OBSTACLES_EMERGENCY_STOP | enable_obstacles_emergency_stop | int | 0/1 | - | Yes | Limiter for PID control on user cartesian command | - | - * | OBSTACLES_EMERGENCY_STOP | max_waiting_time | double | - | - | Yes | Name of the ROS node | e.g. baseControl | - * | OBSTACLES_EMERGENCY_STOP | enable_dynamic_max_distance | int | 0/1 | - | Yes | Name of the topic used to publish the robot footprint | e.g. /footprint | - * | OBSTACLES_EMERGENCY_STOP | max_detection_distance | double | m | - | Yes | Frame to which the footprint is attached | e.g. mobile_base_link | - * | OBSTACLES_EMERGENCY_STOP | min_detection_distance | double | m | - | Yes | Diameter of the robot footprint | e.g. 0.5 | - * | OBSTACLES_AVOIDANCE | enable_obstacles_avoidance | int | 0/1 | - | Yes | Name of the topic used to publish odometry info | e.g. /odometry | - * | OBSTACLES_AVOIDANCE | frontal_blind_angle | double | - | - | Yes | Name of the odometry frame | e.g. odom | - * | OBSTACLES_AVOIDANCE | speed_reduction_factor | double | - | - | Yes | Name of the base frame | e.g. mobile_base_body_link | - * | ROS | rosNodeName | string | - | - | Yes | Name of the ROS node | e.g. /cmd_vel | - * | ROS | useGoalFromRosTopic | bool | - | - | Yes | Name of the topic used to control the robot from ROS | e.g. /robotGoto | - * | ROS | goalTopicName | string | - | - | Yes | Name of the topic used to receive goals from ROS modules | e.g. //move_base_simple/goal | - * | ROS | publishRosStuff | bool | - | - | Yes | Name of the topic used to control the robot from ROS | e.g. /cmd_vel | - * | ROS | currentGoalTopicName | string | - | - | Yes | Name of the topic used to publish the current goal | e.g. /goal | - * | ROS | localPlanTopicName | string | - | - | Yes | Name of the topic used to control the current local plan | e.g. /local_plan | - * | ROS | globalPlanTopicName | string | - | - | Yes | Name of the topic used to control the current global plan | e.g. /global_plan | + * The module can be configured to stop the navigation if an obstacle is detected, or perform obstacle avoidance using an + * APF-based approach. + * A detailed description of configuration parameters available for the module is provided in the README.md file. */ #include diff --git a/src/robotPathPlanner/README.md b/src/robotPathPlanner/README.md index 9bf21030..c9affb0b 100644 --- a/src/robotPathPlanner/README.md +++ b/src/robotPathPlanner/README.md @@ -1,5 +1,5 @@ # robotPathPlanner - This module performs global path-planning by generating a sequence of waypoints to be tracked by a local navigation algorithm. It receives a goal from the user either via RPC command or via yarp iNavigation2D interface and it computes a sequence of waypotints which are sent one by one to a local navigator such as [**robotGoto**](https://github.com/robotology/navigation/tree/master/src/robotGoto). If the local navigation module fails to reach one of these waypoints, the global navigation is aborted too. + This module performs global path-planning by generating a sequence of waypoints to be tracked by a local navigation algorithm. It receives a goal from the user either via RPC command or via yarp iNavigation2D interface and it computes a sequence of waypoints which are sent one by one to a local navigator such as [**robotGoto**](https://github.com/robotology/navigation/tree/master/src/robotGoto). If the local navigation module fails to reach one of these waypoints, the global navigation is aborted too. ## YARP Connections * **/roboPathPlanner/rpc** Standard rpc port diff --git a/src/robotPathPlanner/main.cpp b/src/robotPathPlanner/main.cpp index 643d3e4a..6ba0cf81 100644 --- a/src/robotPathPlanner/main.cpp +++ b/src/robotPathPlanner/main.cpp @@ -16,6 +16,14 @@ * Public License for more details */ +/** + * \section robotPathPlanner + * This module performs global path-planning by generating a sequence of waypoints to be tracked by a local navigation algorithm. + * It receives a goal from the user either via RPC command or via yarp iNavigation2D interface and it computes a sequence of waypoints which are sent one by one to a local navigator module. + * If the local navigation module fails to reach one of these waypoints, the global navigation is aborted too. + * A detailed description of configuration parameters available for the module is provided in the README.md file. + */ + #include #include #include