diff --git a/README.md b/README.md index 819231482e600e7474acf718921a20f3ce9d6955..2a1ce7fef9b82af4473af090b36a812fed3c3de9 100644 --- a/README.md +++ b/README.md @@ -1,16 +1,41 @@ ## Description -The iri_model_car_control project description +This ROS package generates the motion commands to the platform (both forward speed and steering angle) from a standard [Twist](http://docs.ros.org/en/jade/api/geometry_msgs/html/msg/Twist.html) ROS message. + +The forward speed command accepted by the platform is in the range [-100,100] and has no physical magnitude. To generate this command form the input Twist (linear velocity in the x axis) a simple PID controller is used together with the actual speed of the platform taken from the odometry information. The PID parameters can be set by a set of parameters. + +The steering angle command accepted by the platform is also in the range [-100,100] and has no physical magnitude. To generate this command, the desired angular speed in the z axis is transformed to the necessary steering angle to achieve the desired angular speed using the kinematic parameters of the platform. In this case, no feedback is available, so the computed value is directly sent to the platform without any controller. + +This package also implements a watchdog feature in order to stop the platform in case the input motion commands are received peridocally. The value of this watchdog period can be set by a parameter. # ROS Interface ### Topic publishers - - ~**control** (ackermann_msgs/AckermannDriveStamped.msg) + - ~**control** (ackermann_msgs/AckermannDriveStamped.msg) Actual motion commands sent to the platform. + ### Topic subscribers - - ~**cmd_vel** (geometry_msgs/Twist.msg) - - ~**odom** (nav_msgs/Odometry.msg) + - ~**cmd_vel** (geometry_msgs/Twist.msg) Standard velocity command in ROS. + - ~**odom** (nav_msgs/Odometry.msg) Pose and velocities feedback information from the platform. ### Parameters -- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. +- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. +- ~**speed_Kp** (Double; default: 1.0; min: 0.0; max: 1000) Proportional constant of the speed PID. +- ~**speed_Ki** (Double; default: 0.0; min: 0.0; max: 1000) Integral constant of the speed PID. +- ~**speed_Kd** (Double; default: 0.0; min: 0.0; max: 1000) Differential constant of the speed PID. +- ~**speed_i_max** (Double; default: 10.0; min: 0.0; max: 1000) Maximum integral value pf the speed PID. +- ~**watchdog_time** ** (Double; default: 0.5; min: 0.0; max: 1000) Maximum time between two control commands. +- ~**axel_distance** (Double; default: 0.3662; min: 0.01; max: 1.0) Distance between axels in meters. This parameter must coincide with the actual platform value to minimize estimation errors. +- ~**max_steer_angle** (Double; default: 0.4; min: -1.0; max: 1.0) Maximum steering angle. +- ~**min_steer_angle** (Double; default: -0.4; min: -1.0; max: 1.0) Minimum steering angle. +- ~**max_speed_control** (Double; default: 10.0; min: -100.0; max: 100.0) Maximum output speed command value. +- ~**min_speed_control** (Double; default: -10.0; min: -100.0; max: 100.0) Minimum output speed command value. +- ~**max_steer_control** (Double; default: 10.0; min: -100.0; max: 100.0) Maximum output steer command value. +- ~**min_steer_control** (Double; default: -10.0; min: -100.0; max: 100.0) Minimum output steer command value. + + +## Dependencies +This package requires the following packages: + +* [iri_model_car_msgs](https://gitlab.iri.upc.edu/mobile_robotics/adc/platform/model_car/iri_model_car_msgs): Set of specific ROS messages for the model car. ## Installation @@ -18,9 +43,9 @@ Move to the active workspace: ```bash roscd && cd ../src ``` -Clone the repository: +Clone the repository: ```bash -git clone <url> +git clone https://gitlab.iri.upc.edu/mobile_robotics/adc/platform/model_car/iri_model_car_control ``` Install ROS dependencies: ``` @@ -33,13 +58,21 @@ Compile the workspace: catkin_make ``` -## How to use it +## Launch + +This package is normally launched as part of a more complete launch file, either in simulation or with the real robot. See the documentation in [here](https://gitlab.iri.upc.edu/mobile_robotics/adc/platform/model_car/simulator/iri_model_car_how_to) for more information. -- Standalone test +The main launch file (*node.launch*) has the following arguments: - `roslaunch iri_model_car_control test.launch` +* **node_name** (default=model_car_control): name of the ROS node. +* **output** (default=screen): Desired log output. Possible values are screen and log. +* **launch_prefix** (default=): Set of parameters to be pre-apended to the node launch command. +* **config_file** (default=$(find iri_model_car_control)/config/params.yaml): file with the node configuration. See this [section](#parameters) for more information on the available parameters. +* **cmd_vel_topic** (default=/encoders): Name of the velocities command topic to be used. +* **odom_topic** (default=/odom): Name of the odom topic to be used +* **control_topic** (default=/control): Name of the control topic to be used. -## Disclaimer +## Disclaimer Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC. Mantainer IRI labrobotics (labrobotica@iri.upc.edu) diff --git a/cfg/ModelCarControl.cfg b/cfg/ModelCarControl.cfg index 984ef73a15326241bd81955fee613513bdcfe2f5..c7138b3fe1a4177e47829f0b6f86fbd096ceb2d9 100755 --- a/cfg/ModelCarControl.cfg +++ b/cfg/ModelCarControl.cfg @@ -43,7 +43,7 @@ gen.add("speed_Kp", double_t, 0, "Proportional constant of th gen.add("speed_Ki", double_t, 0, "Integral constant of the speed PID", 0.0, 0.0, 1000.0) gen.add("speed_Kd", double_t, 0, "Differential constant of the speed PID", 0.0, 0.0, 1000.0) gen.add("speed_i_max", double_t, 0, "Maximum integral value pf the speed PID", 10.0, 0.0, 1000.0) -gen.add("watchdog_time", double_t, 0, "Maximum time between two control comamnds", 0.5, 0.0, 1000.0) +gen.add("watchdog_time", double_t, 0, "Maximum time between two control commands", 0.5, 0.0, 1000.0) gen.add("axel_distance", double_t, 0, "Distance between the two axels in meters", 0.3662, 0.0, 10.0) gen.add("max_steer_angle", double_t, 0, "Maximum steering angle", 0.4, -1.0,1.0) gen.add("min_steer_angle", double_t, 0, "Minimum steering angle", -0.4, -1.0,1.0)