Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update the plugin to use a PID, also example & docs. #54

Open
wants to merge 1 commit into
base: kinetic-devel
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions hector_gazebo_plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -4,7 +4,7 @@ project(hector_gazebo_plugins)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs geometry_msgs nav_msgs tf dynamic_reconfigure message_generation)
find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs geometry_msgs nav_msgs tf dynamic_reconfigure message_generation control_toolbox)
include_directories(include ${catkin_INCLUDE_DIRS})

## Find gazebo
@@ -56,7 +56,7 @@ generate_dynamic_reconfigure_options(
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf message_runtime
CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf message_runtime control_toolbox
INCLUDE_DIRS include
LIBRARIES
)
187 changes: 187 additions & 0 deletions hector_gazebo_plugins/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,187 @@
# hector_gazebo_plugins
hector_gazebo_plugins provides gazebo plugins from Team Hector. Currently it contains a 6wd differential drive plugin, an IMU sensor plugin, an earth magnetic field sensor plugin, a GPS sensor plugin, a sonar ranger plugin and a plugin to move by using force robots (useful for complicated wheeled robots).

# Available Plugins


## DiffDrivePlugin6W
This plugin serves as a controller for a 6-wheeled light-weight robot using differential drive.

## GazeboRosImu
GazeboRosImu is a replacement for the GazeboRosImu plugin in package gazebo_plugins. It simulates an Inertial Measurement Unit (IMU) affected by Gaussian noise and low-frequency random drift. The orientation returned mimics a simple Attitude and Heading Reference System (AHRS) using the (erroneous) rates and accelerations.

### Published Topics
* imu (`sensor_msgs/Imu`):
The simulated IMU measurements (in body coordinates) and estimated orientation relative to the world frame.

### Advertised Services

* imu/calibrate (std_srvs/Empty):
Resets the offset and drift error of the gyroscopes.
* imu/set_accel_bias (hector_gazebo_plugins/SetBias):
Sets the current offset error of the accelerometers and resets random drift.
* imu/set_gyro_bias (hector_gazebo_plugins/SetBias):
Sets the current offset error of the gyroscopes and resets the random drift.

### XML Parameters

* updateRate (double): the update rate of the sensor in hertz
* robotNamespace (string): namespace prefix for topic and service names
* bodyName (string, required): name of the body which holds the IMU sensor
* topicName (string): name of the sensor output topic and prefix of service names (defaults to imu)
* serviceName (string): name of the calibrate service (for compatibility with gazebo_plugins, defaults to [topicName]/calibrate)
* accelOffset (Vector3): constant acceleration offset
* accelDrift (Vector3): standard deviation of the acceleration drift error
* accelDriftFrequency (Vector3): mean frequency of the acceleration drift
* accelGaussianNoise (Vector3): standard deviation of the additive Gaussian acceleration noise
* rateOffset (Vector3): constant rate offset
* rateDrift (Vector3): standard deviation of the rate drift error
* rateDriftFrequency (Vector3): mean frequency of the rate drift
* rateGaussianNoise (Vector3): standard deviation of the additive Gaussian rate noise
* yawOffset (double): constant yaw/heading offset
* yawDrift (double): standard deviation of the yaw/heading drift error
* yawDriftFrequency (double): mean frequency of the yaw/heading drift
* yawGaussianNoise (double): standard deviation of the yaw/heading additive Gaussian noise
* rpyOffsets (Vector3): if non-zero, used to calculate accelOffset and yawOffset so that resulting roll, pitch and yaw errors correspond to this values when the platform is leveled (for compatibility with gazebo_ plugins)
* gaussianNoise (double): if non-zero, this value is used as standard deviation of Gaussian noise for acceleration and angular rate measurements (for compatibility with gazebo_plugins)


## GazeboRosGps
GazeboRosGps simulates a GNSS (Global Navigation Satellite System) receiver which is attached to a robot. It publishes `sensor_msgs/NavSatFix` messages with the robot's position and altitude in WGS84 coordinates. The reference point that corresponds to the origin of the gazebo frame can be configured using the XML parameters. The conversion between gazebo coordinates and WGS84 is done using a simple equirectangular projection, which is accurate enough if you do not go far away from the configured reference point and if you do not want to use the plugin for polar regions.

### Published Topics

* fix (sensor_msgs/NavSatFix):
The simulated GNSS position in WGS84 coordinates (latitude, longitude and altitude).
* fix_velocity (geometry_msgs/Vector3Stamped):
The GNSS velocity vector in NWU (north, west, up) coordinates.

### XML Parameters

* updateRate (double): the update rate of the sensor in milliseconds
* robotNamespace (string): namespace prefix for topic names
* bodyName (string, required): name of the body the GNSS receiver is attached to
* frameId (string): frame_id included in the message headers (defaults to empty)
* topicName (string): fix output topic (defaults to fix)
* velocityTopicName (string): velocity output topic (defaults to fix_velocity)
* referenceLatitude (double): latitude of the reference point in degrees north (defaults to 49.9)
* referenceLongitude (double): longitude of the reference point in degrees east (defaults to 8.9)
* referenceHeading (double): geographic heading of gazebo frame's x axis in degrees (defaults to 0.0)
* referenceAltitude (double): altitude (height above WGS84 ellipsoid) of the reference point in meters (* defaults to 0.0)
* status (int8): status flag in the fix message (defaults to STATUS_FIX, see sensor_msgs/NavSatStatus)
* service (uint16): service flag in the fix message (defaults to SERVICE_GPS, see sensor_msgs/NavSatStatus)
* offset (Vector3): a constant position offset in meters given in gazebo coordinates
* drift (Vector3): standard deviation of the position drift error
Note: The current velocity error is added to the position drift in each update step. Even if this parameter is zero, the velocity error is also reflected in the position.
* driftFrequency (Vector3): mean frequency of the position drift
* gaussianNoise (Vector3): standard deviation of the additive Gaussian noise added to the position
* velocityOffset (Vector3): constant velocity offset in meters/seconds given in gazebo coordinates
* velocityDrift (Vector3): standard deviation of the velocity error
* velocityDriftFrequency (Vector3): mean frequency of the velocity drift
* velocityGaussianNoise (Vector3): standard deviation of the additive Gaussian noise added to the velocity


## GazeboRosMagnetic
This plugin simulates a 3-axis magnetometer like PNI Corp's MicroMag. Like for the GPS plugin, the orientation of the gazebo frame can be specified via the referenceHeading parameter. Declination, inclination and field magnitude have to be configured depending on your location on earth. The default parameters are valid for Europe and North America without magnitude information (normalized vector). Check http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml for exact parameters.

### Published Topics

* magnetic (geometry_msgs/Vector3Stamped):
The magnetic field vector given in body coordinates.

### XML Parameters

* updateRate (double): the update rate of the sensor in milliseconds
* robotNamespace (string): namespace prefix for topic names
* bodyName (string, required): name of the body the sensor is attached to
* topicName (string): fix output topic (defaults to /fix)
* magnitude (double): magnitude of the magnetic field in whatever unit you want (defaults to 1.0)
* referenceHeading (double): geographic heading of gazebo frame's x axis in degrees (defaults to 0.0)
* declination (double): declination (angle between true and magnetic north) in degrees (defaults to 0.0)
* inclination (double): inclination (angle between the horizontal plane and the magnetic field) in degrees (* defaults to 60.0)
* offset (Vector3): a constant offset added to the magnetic field vector. This parameter can be used to simulate deviation caused by magnetized iron within the robot.
* drift (Vector3): standard deviation of the drift error
* driftFrequency (Vector3): mean frequency of the drift
* gaussianNoise (Vector3): standard deviation of the additive Gaussian noise added to the magnetic field


## GazeboRosSonar
The GazeboRosSonar plugin is a ROS controller for gazebo's built-in ray sensor. The value returned as sonar range is the minimum of all rays, as a sonar ranger returns the distance corresponding to the first echo returned from a object within it's field of view. The behavior of this controller plugin depends mainly on the parameters of the sensor it is attached to.

The <controller:hector_gazebo_ros_sonar> tag is only valid within a surrounding <sensor:ray> tag. You should use the macro defined in sonar_sensor.urdf.xacro in package hector_sensors_description to include a sonar sensor to your model.

### Published Topics

* sonar (sensor_msgs/Range):
A Range message containing the minimum distance of all rays. The field_of_view, min_range and max_range values reflect the parameters of the ray sensor.

### XML Parameters

* updateRate (double): the update rate of the sensor in milliseconds, should be equal to the update rate of the containing sensor's updateRate
* robotNamespace (string): namespace prefix for topic names
* frameId (string): frame_id included in the message header
* topicName (string): name of the sensor output topic (defaults to sonar)
* offset (double): a constant offset added to the range
* drift (double): standard deviation of the drift error
* driftFrequency (double): mean frequency of the drift
* gaussianNoise (double): standard deviation of the additive Gaussian noise


## GazeboRosForceBasedMove
The GazeboRosForceBasedMove plugin is a ROS controller that uses a `geometry_msgs/Twist` message to exert
forces on a robot, resulting in motion. Based on the planar_move plugin by Piyush Khandelwal. Useful for robots with complicated wheeled based that are hard(er) to simulate otherwise. For example, three wheeled holonomic bases.

You'll need to configure the wheels as static links (or fixed joints) and give them reasonable mu, mu2 and fdir gazebo parameters.

You can find an example of a simple robot in `roslaunch hector_gazebo_plugins boxbot_example.launch`.

![gif boxbot](force_move_plugin.gif).


### Subscribed Topics
* cmd_vel: `geometry_msgs/Twist` command topic.

### Published Topics
* odom: `nav_msgs/Odometry` of the robot. (Optional)


### XML Parameters

* robotNamespace (string): namespace prefix for topic names. Defaults to '' (empty).
* commandTopic (string): topic to listen for `geometry_msgs/Twist` messages, typically `cmd_vel`. Defaults to `cmd_vel`.
* odometryTopic (string): topic where to publish the `nav_msgs/Odometry` odometry of the robot. Defaults to `odom`
* odometryFrame (string): frame_id included in the message header of Odometry messages. Defaults to `odom`.
* odometryRate (double): rate (Hz) to publish odometry, defaults to 20.0.
* publishOdometryTf (bool): if to publish the odometry in TF, defaults to **true**.
* robotBaseFrame (string): base frame on to where apply the forces to move, defaults to `base_footprint`.

Note that all the PIDs are available as a dynamic_reconfigure server (at your_namespace/force_based_move/[x/y/yaw]\_pid) to tune on the fly, so once you find useful values you can write them down in the SDF.
They are highly correlated to your mu, mu2 and fdir variables and the mass of the robot.
The PIDs are implemented via the `control_toolbox::Pid` class.

* yaw_velocity_p_gain (double): Proportional gain to apply to yaw. Defaults to 0.0.
* yaw_velocity_i_gain (double): Integrative gain to apply to yaw. Defaults to 0.0.
* yaw_velocity_d_gain (double): Derivative gain to apply to yaw. Defaults to 0.0.
* yaw_velocity_i_clamp (double): Clamping of integrative gain value. Defaults to 0.0.
* x_velocity_p_gain (double): Proportional gain to apply to x. Defaults to 0.0.
* x_velocity_i_gain (double): Integrative gain to apply to x. Defaults to 0.0.
* x_velocity_d_gain (double): Derivative gain to apply to x. Defaults to 0.0.
* x_velocity_i_clamp (double): Clamping of integrative gain value. Defaults to 0.0.
* y_velocity_p_gain (double): Proportional gain to apply to y. Defaults to 0.0.
* y_velocity_i_gain (double): Integrative gain to apply to y. Defaults to 0.0.
* y_velocity_d_gain (double): Derivative gain to apply to y. Defaults to 0.0.
* y_velocity_i_clamp (double): Clamping of integrative gain value. Defaults to 0.0.



# Sensor Error Model
All simulated sensors from this package use the same generic model for sensor errors like

* constant offset
* low-frequency drift (modeled as a first order Markov random process with defined mean frequency and mean standard deviation)
* gaussian noise (with defined standard deviation).
The parameters are initialized to 0 by default (no sensor error). The default drift frequency is 0.00027 Hz (one hour period).

# See also
You can find some more plugins useful for aerial vehicles in package [hector_quadrotor_gazebo_plugins](http://wiki.ros.org/hector_quadrotor_gazebo_plugins).

Binary file added hector_gazebo_plugins/force_move_plugin.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
@@ -19,8 +19,8 @@
* Desc: Simple model controller that uses a twist message to exert
* forces on a robot, resulting in motion. Based on the
* planar_move plugin by Piyush Khandelwal.
* Author: Stefan Kohlbrecher
* Date: 06 August 2015
* Author: Stefan Kohlbrecher, Sammy Pfeiffer
* Date: 06 August 2015, 21 December 2018
*/

#ifndef GAZEBO_ROS_FORCE_BASED_MOVE_HH
@@ -42,6 +42,7 @@
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <control_toolbox/pid.h>

namespace gazebo {

@@ -59,7 +60,7 @@ namespace gazebo {
private:
void publishOdometry(double step_time);

tf::Transform getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const;
tf::Transform getTransformForMotion(double linear_vel_x, double linear_vel_y, double angular_vel, double timeSeconds) const;

physics::ModelPtr parent_;
event::ConnectionPtr update_connection_;
@@ -97,20 +98,35 @@ namespace gazebo {
// command velocity callback
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);

// latest command
double x_;
double y_;
double rot_;
double yaw_;

bool alive_;
common::Time last_odom_publish_time_;
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Pose3d last_odom_pose_;
#else
math::Pose last_odom_pose_;
#endif

control_toolbox::Pid pid_yaw_velocity_;
control_toolbox::Pid pid_x_velocity_;
control_toolbox::Pid pid_y_velocity_;
ros::Time last_pid_update_time_;

double torque_yaw_velocity_p_gain_;
double force_x_velocity_p_gain_;
double force_y_velocity_p_gain_;
double torque_yaw_velocity_i_gain_;
double force_x_velocity_i_gain_;
double force_y_velocity_i_gain_;
double torque_yaw_velocity_d_gain_;
double force_x_velocity_d_gain_;
double force_y_velocity_d_gain_;
double torque_yaw_velocity_i_clamp_;
double force_x_velocity_i_clamp_;
double force_y_velocity_i_clamp_;

};

44 changes: 44 additions & 0 deletions hector_gazebo_plugins/launch/boxbot.gazebo
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
<?xml version="1.0"?>
<robot>

<!-- Link1 -->
<gazebo reference="link1">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="10000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
<minDepth>0.003</minDepth>
<turnGravityOff>false</turnGravityOff>
<material>Gazebo/Orange</material>
</gazebo>

<!-- Link2 -->
<gazebo reference="link2">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>

<gazebo>
<plugin name="base_drive_controller" filename="libgazebo_ros_force_based_move.so">
<robotNamespace>boxbot</robotNamespace>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<publishOdometryTf>true</publishOdometryTf>
<odometryFrame>odom</odometryFrame>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>link1</robotBaseFrame>
<yaw_velocity_p_gain>100.0</yaw_velocity_p_gain>
<yaw_velocity_i_gain>0.0</yaw_velocity_i_gain>
<yaw_velocity_i_clamp>00.0</yaw_velocity_i_clamp>
<x_velocity_p_gain>1000.0</x_velocity_p_gain>
<x_velocity_i_gain>0.0</x_velocity_i_gain>
<x_velocity_i_clamp>0.0</x_velocity_i_clamp>
<y_velocity_p_gain>1000.0</y_velocity_p_gain>
<y_velocity_i_gain>0.0</y_velocity_i_gain>
<y_velocity_i_clamp>0.0</y_velocity_i_clamp>
</plugin>
</gazebo>

</robot>
66 changes: 66 additions & 0 deletions hector_gazebo_plugins/launch/boxbot.urdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from boxbot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!-- Revolute-Revolute Manipulator -->
<robot name="boxbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Link 1 -->
<!-- Link1 -->
<gazebo reference="link1">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<minDepth>0.003</minDepth>
<turnGravityOff>false</turnGravityOff>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_force_based_move.so" name="base_drive_controller">
<robotNamespace/>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<publishOdometryTf>true</publishOdometryTf>
<odometryFrame>odom</odometryFrame>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>link1</robotBaseFrame>
<!-- all these can be changed in dynamic reconfigure -->
<yaw_velocity_p_gain>0.0</yaw_velocity_p_gain>
<yaw_velocity_i_gain>0.0</yaw_velocity_i_gain>
<yaw_velocity_i_clamp>00.0</yaw_velocity_i_clamp>
<x_velocity_p_gain>0.0</x_velocity_p_gain>
<x_velocity_i_gain>0.0</x_velocity_i_gain>
<x_velocity_i_clamp>0.0</x_velocity_i_clamp>
<y_velocity_p_gain>0.0</y_velocity_p_gain>
<y_velocity_i_gain>0.0</y_velocity_i_gain>
<y_velocity_i_clamp>0.0</y_velocity_i_clamp>
</plugin>
</gazebo>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<!-- Base Link -->
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<geometry>
<box size="0.5 0.5 0.3"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<geometry>
<box size="0.5 0.5 0.3"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<mass value="5"/>
<inertia ixx="0.141666666667" ixy="0.0" ixz="0.0" iyy="0.141666666667" iyz="0.0" izz="0.208333333333"/>
</inertial>
</link>
</robot>

76 changes: 76 additions & 0 deletions hector_gazebo_plugins/launch/boxbot.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="boxbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="mass" value="5" /> <!-- arbitrary value for mass -->
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="height1" value="0.3" /> <!-- Link 1 -->

<!-- Import all Gazebo-customization elements, including Gazebo colors -->
<xacro:include filename="$(find hector_gazebo_plugins)/launch/boxbot.gazebo" />
<!-- Import Rviz colors -->
<xacro:include filename="$(find hector_gazebo_plugins)/launch/materials.xacro" />

<!-- Base Link -->
<link name="link1">
<collision>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
</collision>

<visual>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
<material name="orange"/>
</visual>

<inertial>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>

<joint name="joint1" type="fixed">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="${width/4} 0 ${height1*1.25}" rpy="0 0 0"/>
</joint>

<!-- Box acting as front side -->
<link name="link2">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.2"/>
</geometry>
</collision>

<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.2"/>
</geometry>
<material name="orange"/>
</visual>

<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="${0.1}"/>
<inertia
ixx="${mass/2 / 12.0 * (0.2*0.2 + 0.2*0.2)}" ixy="0.0" ixz="0.0"
iyy="${mass/2 / 12.0 * (0.2*0.2 + 0.2*0.2)}" iyz="0.0"
izz="${mass/2 / 12.0 * (0.2*0.2 + 0.2*0.2)}"/>
</inertial>
</link>

</robot>
28 changes: 28 additions & 0 deletions hector_gazebo_plugins/launch/boxbot_example.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<launch>

<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>

<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rrbot_gazebo)/worlds/rrbot.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>

<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find hector_gazebo_plugins)/launch/boxbot.xacro'" />

<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model boxbot -param robot_description"/>

</launch>
8 changes: 8 additions & 0 deletions hector_gazebo_plugins/launch/materials.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
<?xml version="1.0"?>
<robot>

<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>

</robot>
6 changes: 4 additions & 2 deletions hector_gazebo_plugins/package.xml
Original file line number Diff line number Diff line change
@@ -3,8 +3,8 @@
<version>0.5.1</version>
<description>hector_gazebo_plugins provides gazebo plugins from Team Hector.
Currently it contains a 6wd differential drive plugin, an IMU sensor plugin,
an earth magnetic field sensor plugin, a GPS sensor plugin and a
sonar ranger plugin.</description>
an earth magnetic field sensor plugin, a GPS sensor plugin,
sonar ranger plugin and a force based planar move plugin.</description>
<maintainer email="johannes@intermodalics.eu">Johannes Meyer</maintainer>

<license>BSD</license>
@@ -28,6 +28,7 @@
<build_depend>tf</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>control_toolbox</build_depend>

<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
@@ -40,5 +41,6 @@
<run_depend>tf</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>control_toolbox</run_depend>

</package>
159 changes: 119 additions & 40 deletions hector_gazebo_plugins/src/gazebo_ros_force_based_move.cpp
Original file line number Diff line number Diff line change
@@ -19,8 +19,8 @@
* Desc: Simple model controller that uses a twist message to exert
* forces on a robot, resulting in motion. Based on the
* planar_move plugin by Piyush Khandelwal.
* Author: Stefan Kohlbrecher
* Date: 06 August 2015
* Author: Stefan Kohlbrecher, Sammy Pfeiffer
* Date: 06 August 2015, 21 December 2018
*/

#include <hector_gazebo_plugins/gazebo_ros_force_based_move.h>
@@ -30,7 +30,9 @@ namespace gazebo

GazeboRosForceBasedMove::GazeboRosForceBasedMove() {}

GazeboRosForceBasedMove::~GazeboRosForceBasedMove() {}
GazeboRosForceBasedMove::~GazeboRosForceBasedMove() {
rosnode_->shutdown();
}

// Load the controller
void GazeboRosForceBasedMove::Load(physics::ModelPtr parent,
@@ -89,10 +91,21 @@ namespace gazebo
odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
}


torque_yaw_velocity_p_gain_ = 100.0;
force_x_velocity_p_gain_ = 10000.0;
force_y_velocity_p_gain_ = 10000.0;
// All these are related somehow to the friction
// coefficients of the link that we are pushing with the force
// as in, mu, mu2, fdir
torque_yaw_velocity_p_gain_ = 0.0;
force_x_velocity_p_gain_ = 0.0;
force_y_velocity_p_gain_ = 0.0;
torque_yaw_velocity_i_gain_ = 0.0;
force_x_velocity_i_gain_ = 0.0;
force_y_velocity_i_gain_ = 0.0;
torque_yaw_velocity_d_gain_ = 0.0;
force_x_velocity_d_gain_ = 0.0;
force_y_velocity_d_gain_ = 0.0;
torque_yaw_velocity_i_clamp_ = 0.0;
force_x_velocity_i_clamp_ = 0.0;
force_y_velocity_i_clamp_ = 0.0;

if (sdf->HasElement("yaw_velocity_p_gain"))
(sdf->GetElement("yaw_velocity_p_gain")->GetValue()->Get(torque_yaw_velocity_p_gain_));
@@ -102,10 +115,47 @@ namespace gazebo

if (sdf->HasElement("y_velocity_p_gain"))
(sdf->GetElement("y_velocity_p_gain")->GetValue()->Get(force_y_velocity_p_gain_));

if (sdf->HasElement("yaw_velocity_i_gain"))
(sdf->GetElement("yaw_velocity_i_gain")->GetValue()->Get(torque_yaw_velocity_i_gain_));

if (sdf->HasElement("x_velocity_i_gain"))
(sdf->GetElement("x_velocity_i_gain")->GetValue()->Get(force_x_velocity_i_gain_));

if (sdf->HasElement("y_velocity_i_gain"))
(sdf->GetElement("y_velocity_i_gain")->GetValue()->Get(force_y_velocity_i_gain_));

if (sdf->HasElement("yaw_velocity_d_gain"))
(sdf->GetElement("yaw_velocity_d_gain")->GetValue()->Get(torque_yaw_velocity_d_gain_));

if (sdf->HasElement("x_velocity_d_gain"))
(sdf->GetElement("x_velocity_d_gain")->GetValue()->Get(force_x_velocity_d_gain_));

if (sdf->HasElement("y_velocity_d_gain"))
(sdf->GetElement("y_velocity_d_gain")->GetValue()->Get(force_y_velocity_d_gain_));

if (sdf->HasElement("yaw_velocity_i_clamp"))
(sdf->GetElement("yaw_velocity_i_clamp")->GetValue()->Get(torque_yaw_velocity_i_clamp_));

if (sdf->HasElement("x_velocity_i_clamp"))
(sdf->GetElement("x_velocity_i_clamp")->GetValue()->Get(force_x_velocity_i_clamp_));

if (sdf->HasElement("y_velocity_i_clamp"))
(sdf->GetElement("y_velocity_i_clamp")->GetValue()->Get(force_y_velocity_i_clamp_));


ROS_INFO_STREAM("ForceBasedMove using gains: yaw: " << torque_yaw_velocity_p_gain_ <<
" x: " << force_x_velocity_p_gain_ <<
" y: " << force_y_velocity_p_gain_ << "\n");
ROS_INFO_STREAM("ForceBasedMove using gains:\np yaw: " << torque_yaw_velocity_p_gain_ <<
"\np x: " << force_x_velocity_p_gain_ <<
"\np y: " << force_y_velocity_p_gain_ <<
"\ni yaw: " << torque_yaw_velocity_i_gain_ <<
"\ni x: " << force_x_velocity_i_gain_ <<
"\ni y: " << force_y_velocity_i_gain_ <<
"\nd yaw: " << torque_yaw_velocity_d_gain_ <<
"\nd x: " << force_x_velocity_d_gain_ <<
"\nd y: " << force_y_velocity_d_gain_ <<
"\ni_clamp yaw: " << torque_yaw_velocity_i_clamp_ <<
"\ni_clamp x: " << force_x_velocity_i_clamp_ <<
"\ni_clamp y: " << force_y_velocity_i_clamp_);

robot_base_frame_ = "base_footprint";
if (!sdf->HasElement("robotBaseFrame"))
@@ -150,9 +200,10 @@ namespace gazebo
last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
last_odom_pose_ = parent_->GetWorldPose();
#endif
x_ = 0;
y_ = 0;
rot_ = 0;

x_ = 0.0;
y_ = 0.0;
yaw_ = 0.0;
alive_ = true;

odom_transform_.setIdentity();
@@ -168,7 +219,7 @@ namespace gazebo
}
rosnode_.reset(new ros::NodeHandle(robot_namespace_));

ROS_DEBUG("OCPlugin (%s) has started!",
ROS_DEBUG("ForceBasedMove Plugin (%s) has started!",
robot_namespace_.c_str());

tf_prefix_ = tf::getPrefixParam(*rosnode_);
@@ -194,51 +245,72 @@ namespace gazebo
event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosForceBasedMove::UpdateChild, this));

// Apparently the dynamic reconfigure server only instances if there is at least
// a 'p' param for it
rosnode_->setParam("force_based_move/yaw_pid/p", torque_yaw_velocity_p_gain_);
rosnode_->setParam("force_based_move/yaw_pid/i", torque_yaw_velocity_i_gain_);
rosnode_->setParam("force_based_move/yaw_pid/d", torque_yaw_velocity_d_gain_);
rosnode_->setParam("force_based_move/yaw_pid/i_clamp_min", -torque_yaw_velocity_i_clamp_);
rosnode_->setParam("force_based_move/yaw_pid/i_clamp_max", torque_yaw_velocity_i_clamp_);
pid_yaw_velocity_.init(ros::NodeHandle(*rosnode_, "force_based_move/yaw_pid"), false);

rosnode_->setParam("force_based_move/x_pid/p", force_x_velocity_p_gain_);
rosnode_->setParam("force_based_move/x_pid/i", force_x_velocity_i_gain_);
rosnode_->setParam("force_based_move/x_pid/d", force_x_velocity_d_gain_);
rosnode_->setParam("force_based_move/x_pid/i_clamp_min", -force_x_velocity_i_clamp_);
rosnode_->setParam("force_based_move/x_pid/i_clamp_max", force_x_velocity_i_clamp_);
pid_x_velocity_.init(ros::NodeHandle(*rosnode_, "force_based_move/x_pid"), false);

rosnode_->setParam("force_based_move/y_pid/p", force_y_velocity_p_gain_);
rosnode_->setParam("force_based_move/y_pid/i", force_y_velocity_i_gain_);
rosnode_->setParam("force_based_move/y_pid/d", force_y_velocity_d_gain_);
rosnode_->setParam("force_based_move/y_pid/i_clamp_min", -force_y_velocity_i_clamp_);
rosnode_->setParam("force_based_move/y_pid/i_clamp_max", force_y_velocity_i_clamp_);
pid_y_velocity_.init(ros::NodeHandle(*rosnode_, "force_based_move/y_pid"), false);
}


// Update the controller
void GazeboRosForceBasedMove::UpdateChild()
{
boost::mutex::scoped_lock scoped_lock(lock);
ros::Time tnow = ros::Time::now();
ros::Duration dt = tnow - last_pid_update_time_;
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Pose3d pose = parent_->WorldPose();

ignition::math::Vector3d angular_vel = parent_->WorldAngularVel();

double error = angular_vel.Z() - rot_;
double value_yaw = pid_yaw_velocity_.computeCommand(pid_yaw_velocity_.getCurrentCmd() - angular_vel.Z(), dt);

link_->AddTorque(ignition::math::Vector3d(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
link_->AddTorque(ignition::math::Vector3d(0.0, 0.0, value_yaw));

float yaw = pose.Rot().Yaw();

ignition::math::Vector3d linear_vel = parent_->RelativeLinearVel();

link_->AddRelativeForce(ignition::math::Vector3d((x_ - linear_vel.X())* force_x_velocity_p_gain_,
(y_ - linear_vel.Y())* force_y_velocity_p_gain_,
double value_x = pid_x_velocity_.computeCommand(pid_x_velocity_.getCurrentCmd() - linear_vel.X(), dt);
double value_y = pid_y_velocity_.computeCommand(pid_y_velocity_.getCurrentCmd() - linear_vel.Y(), dt);

link_->AddRelativeForce(ignition::math::Vector3d(value_x,
value_y,
0.0));
#else
math::Pose pose = parent_->GetWorldPose();

math::Vector3 angular_vel = parent_->GetWorldAngularVel();
math::Vector3 linear_vel = parent_->GetRelativeLinearVel();

double error = angular_vel.z - rot_;

link_->AddTorque(math::Vector3(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
double value_yaw = pid_yaw_velocity_.computeCommand(yaw_ - angular_vel.z, dt);
link_->AddTorque(math::Vector3(0.0, 0.0, value_yaw));

float yaw = pose.rot.GetYaw();
double value_x = pid_x_velocity_.computeCommand(x_ - linear_vel.x, dt);
double value_y = pid_y_velocity_.computeCommand(y_ - linear_vel.y, dt);

math::Vector3 linear_vel = parent_->GetRelativeLinearVel();

link_->AddRelativeForce(math::Vector3((x_ - linear_vel.x)* force_x_velocity_p_gain_,
(y_ - linear_vel.y)* force_y_velocity_p_gain_,
0.0));
link_->AddRelativeForce(math::Vector3(value_x,
value_y,
0.0));
#endif
//parent_->PlaceOnNearestEntityBelow();
//parent_->SetLinearVel(math::Vector3(
// x_ * cosf(yaw) - y_ * sinf(yaw),
// y_ * cosf(yaw) + x_ * sinf(yaw),
// 0));
//parent_->SetAngularVel(math::Vector3(0, 0, rot_));
last_pid_update_time_ = tnow;

if (odometry_rate_ > 0.0) {
#if (GAZEBO_MAJOR_VERSION >= 8)
@@ -270,7 +342,12 @@ namespace gazebo
boost::mutex::scoped_lock scoped_lock(lock);
x_ = cmd_msg->linear.x;
y_ = cmd_msg->linear.y;
rot_ = cmd_msg->angular.z;
yaw_ = cmd_msg->angular.z;
ROS_DEBUG_STREAM("Updating command:\n" <<
"x: " << cmd_msg->linear.x <<
"\ny: " << cmd_msg->linear.y <<
"\nyaw: " << cmd_msg->angular.z);

}

void GazeboRosForceBasedMove::QueueThread()
@@ -294,20 +371,22 @@ namespace gazebo
ignition::math::Vector3d angular_vel = parent_->RelativeAngularVel();
ignition::math::Vector3d linear_vel = parent_->RelativeLinearVel();

odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.X(), angular_vel.Z(), step_time);
odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.X(), linear_vel.Y(), angular_vel.Z(), step_time);

tf::poseTFToMsg(odom_transform_, odom_.pose.pose);
odom_.twist.twist.angular.z = angular_vel.Z();
odom_.twist.twist.linear.x = linear_vel.X();
odom_.twist.twist.linear.y = linear_vel.Y();
#else
math::Vector3 angular_vel = parent_->GetRelativeAngularVel();
math::Vector3 linear_vel = parent_->GetRelativeLinearVel();

odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.x, angular_vel.z, step_time);
odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.x, linear_vel.y, angular_vel.z, step_time);

tf::poseTFToMsg(odom_transform_, odom_.pose.pose);
odom_.twist.twist.angular.z = angular_vel.z;
odom_.twist.twist.linear.x = linear_vel.x;
odom_.twist.twist.linear.y = linear_vel.y;
#endif

odom_.header.stamp = current_time;
@@ -358,18 +437,18 @@ namespace gazebo
}


tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double linear_vel_y, double angular_vel, double timeSeconds) const
{
tf::Transform tmp;
tmp.setIdentity();


if (std::abs(angular_vel) < 0.0001) {
//Drive straight
tmp.setOrigin(tf::Vector3(static_cast<double>(linear_vel_x*timeSeconds), 0.0, 0.0));
tmp.setOrigin(tf::Vector3(static_cast<double>(linear_vel_x*timeSeconds), static_cast<double>(linear_vel_y*timeSeconds), 0.0));
} else {
//Follow circular arc
double distChange = linear_vel_x * timeSeconds;
double distChange = linear_vel_x * timeSeconds + linear_vel_y * timeSeconds;
double angleChange = angular_vel * timeSeconds;

double arcRadius = distChange / angleChange;