ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi,
We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.
So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?
Thanks
2 | No.2 Revision |
Hi,
We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.
So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?
Thanks
I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :
<arg name="robot_name" default="turtlebot" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="$(arg robot_name)/odom"/>
…
</node>
Now we get this tree :
map
| |
turtlebot_1/odom turtlebot_2/odom
and
odom
|
base_footprint
But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.
3 | No.3 Revision |
Hi,
We get same problem here and we don't know what we miss... We tried with and without tf_prefix (some people say this is not compatible with Hydro). In rviz we can't have the correct position of robot on map.
So the question is : How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint on Hydro ?
Solution 1 :
I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :
<arg name="robot_name" default="turtlebot" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="$(arg robot_name)/odom"/>
…
</node>
Now we get this tree :
map
| |
turtlebot_1/odom turtlebot_2/odom
and
odom
|
base_footprint
But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.
That's it I found !!
So it's the gazebo plugin's of kobuki ros which it's does not take the namespace of robot. To get two branches robot1/odom -> robot1/basefootprint and robot2/odom -> robot2/basefootprint we need to edit the plugin.
I don't know yet how I can tell to gazebo to use the modified plugin so I make a backup :
mv /opt/ros/hydro/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so.old
Then in my Ros workspace I clone the Git repot :
cd ros_workspace/src
git clone h t t p s : //github.com/yujinrobot/kobuki_desktop.git
Once you have all file you can edit the file in
ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
For every publisher and subscriber I add
XXX.subscribe(node_name_ + "/XXXX");
Below all the edited file :
/**
* @author Marcus Liebhardt
*
* This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
*/
#include <cmath>
#include <cstring>
#include <boost/bind.hpp>
#include <sensor_msgs/JointState.h>
#include <tf/LinearMath/Quaternion.h>
#include <gazebo/math/gzmath.hh>
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"
namespace gazebo
{
enum {LEFT= 0, RIGHT=1};
GazeboRosKobuki::GazeboRosKobuki() : shutdown_requested_(false)
{
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// Initialise variables
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
cliff_detected_left_ = false;
cliff_detected_center_ = false;
cliff_detected_right_ = false;
}
GazeboRosKobuki::~GazeboRosKobuki()
{
// rosnode_->shutdown();
shutdown_requested_ = true;
// Wait for spinner thread to end
// ros_spinner_thread_->join();
// delete spinner_thread_;
// delete rosnode_;
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_)
{
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->Get<std::string>("name");
gzdbg << "Plugin model name: " << model_name << "\n";
nh_ = ros::NodeHandle("");
// creating a private name pace until Gazebo implements topic remappings
nh_priv_ = ros::NodeHandle("/" + model_name);
node_name_ = model_name;
world_ = parent->GetWorld();
/*
* Prepare receiving motor power commands
*/
motor_power_sub_ = nh_priv_.subscribe(node_name_ + "/commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
motors_enabled_ = true;
/*
* Prepare joint state publishing
*/
if (sdf->HasElement("left_wheel_joint_name"))
{
left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("right_wheel_joint_name"))
{
right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
if (!joints_[LEFT] || !joints_[RIGHT])
{
ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
return;
}
joint_state_.header.frame_id = "Joint States";
joint_state_.name.push_back(left_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_.name.push_back(right_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(node_name_ + "/joint_states", 1);
/*
* Prepare publishing odometry data
*/
if (sdf->HasElement("publish_tf"))
{
publish_tf_ = sdf->GetElement("publish_tf")->Get<bool>();
if (publish_tf_)
{
ROS_INFO_STREAM("Will publish tf." << " [" << node_name_ <<"]");
}
else
{
ROS_INFO_STREAM("Won't publish tf." << " [" << node_name_ <<"]");
}
}
else
{
publish_tf_ = false;
ROS_INFO_STREAM("Couldn't find the 'publish tf' parameter in the model description."
<< " Won't publish tf." << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_separation"))
{
wheel_sep_ = sdf->GetElement("wheel_separation")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_diameter"))
{
wheel_diam_ = sdf->GetElement("wheel_diameter")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("torque"))
{
torque_ = sdf->GetElement("torque")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
odom_pub_ = nh_.advertise<nav_msgs::Odometry>(node_name_ + "/odom", 1);
odom_reset_sub_ = nh_priv_.subscribe(node_name_ + "/commands/reset_odometry", 10, &GazeboRosKobuki::resetOdomCB, this);
/*
* Prepare receiving velocity commands
*/
if (sdf->HasElement("velocity_command_timeout"))
{
cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
last_cmd_vel_time_ = world_->GetSimTime();
cmd_vel_sub_ = nh_priv_.subscribe(node_name_ + "/commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);
/*
* Prepare cliff sensors
*/
std::string cliff_sensor_left_name, cliff_sensor_center_name, cliff_sensor_right_name;
if (sdf->HasElement("cliff_sensor_left_name"))
{
cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_center_name"))
{
cliff_sensor_center_name = sdf->GetElement("cliff_sensor_center_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_right_name"))
{
cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
cliff_sensor_center_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name));
cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
if (!cliff_sensor_left_)
{
ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_center_)
{
ROS_ERROR_STREAM("Couldn't find the center cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_right_)
{
ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_detection_threshold"))
{
cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_->SetActive(true);
cliff_sensor_center_->SetActive(true);
cliff_sensor_right_->SetActive(true);
cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>(node_name_ + "/events/cliff", 1);
/*
* Prepare bumper
*/
std::string bumper_name;
if (sdf->HasElement("bumper_name"))
{
bumper_name = sdf->GetElement("bumper_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
sensors::SensorManager::Instance()->GetSensor(bumper_name));
if (!bumper_)
{
ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
return;
}
bumper_->SetActive(true);
bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>(node_name_ + "/events/bumper", 1);
/*
* Prepare IMU
*/
std::string imu_name;
if (sdf->HasElement("imu_name"))
{
imu_name = sdf->GetElement("imu_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of IMU sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
imu_ = boost::shared_dynamic_cast<sensors::ImuSensor>(
sensors::SensorManager::Instance()->GetSensor(imu_name));
if (!imu_)
{
ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");
return;
}
imu_->SetActive(true);
imu_pub_ = nh_priv_.advertise<sensor_msgs::Imu>(node_name_ + "/sensors/imu_data", 1);
prev_update_time_ = world_->GetSimTime();
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
void GazeboRosKobuki::motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg)
{
if ((msg->state == kobuki_msgs::MotorPower::ON) && (!motors_enabled_))
{
motors_enabled_ = true;
ROS_INFO_STREAM("Motors fired up. [" << node_name_ << "]");
}
else if ((msg->state == kobuki_msgs::MotorPower::OFF) && (motors_enabled_))
{
motors_enabled_ = false;
ROS_INFO_STREAM("Motors taking a rest. [" << node_name_ << "]");
}
}
void GazeboRosKobuki::cmdVelCB(const geometry_msgs::TwistConstPtr &msg)
{
last_cmd_vel_time_ = world_->GetSimTime();
wheel_speed_cmd_[LEFT] = msg->linear.x - msg->angular.z * (wheel_sep_) / 2;
wheel_speed_cmd_[RIGHT] = msg->linear.x + msg->angular.z * (wheel_sep_) / 2;
}
void GazeboRosKobuki::resetOdomCB(const std_msgs::EmptyConstPtr &msg)
{
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
void GazeboRosKobuki::OnUpdate()
{
/*
* First process ROS callbacks
*/
ros::spinOnce();
/*
* Update current time and time step
*/
common::Time time_now = world_->GetSimTime();
common::Time step_time = time_now - prev_update_time_;
prev_update_time_ = time_now;
/*
* Joint states
*/
joint_state_.header.stamp = ros::Time::now();
joint_state_.header.frame_id = node_name_ + "/base_link";
joint_state_.position[LEFT] = joints_[LEFT]->GetAngle(0).Radian();
joint_state_.velocity[LEFT] = joints_[LEFT]->GetVelocity(0);
joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
joint_state_.velocity[RIGHT] = joints_[RIGHT]->GetVelocity(0);
joint_state_pub_.publish(joint_state_);
/*
* Odometry (encoders & IMU)
*/
odom_.header.stamp = joint_state_.header.stamp;
odom_.header.frame_id = node_name_ + "/odom";
odom_.child_frame_id = node_name_ + "/base_footprint";
// Distance travelled by main wheels
double d1, d2;
double dr, da;
d1 = d2 = 0;
dr = da = 0;
d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
// Can see NaN values here, just zero them out if needed
if (isnan(d1))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
d1 = 0;
}
if (isnan(d2))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
d2 = 0;
}
dr = (d1 + d2) / 2;
da = (d2 - d1) / wheel_sep_; // ignored
// Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
vel_angular_ = imu_->GetAngularVelocity();
// Compute odometric pose
odom_pose_[0] += dr * cos( odom_pose_[2] );
odom_pose_[1] += dr * sin( odom_pose_[2] );
odom_pose_[2] += vel_angular_.z * step_time.Double();
// Compute odometric instantaneous velocity
odom_vel_[0] = dr / step_time.Double();
odom_vel_[1] = 0.0;
odom_vel_[2] = vel_angular_.z;
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
tf::Quaternion qt;
qt.setEuler(0,0,odom_pose_[2]);
odom_.pose.pose.orientation.x = qt.getX();
odom_.pose.pose.orientation.y = qt.getY();
odom_.pose.pose.orientation.z = qt.getZ();
odom_.pose.pose.orientation.w = qt.getW();
odom_.pose.covariance[0] = 0.1;
odom_.pose.covariance[7] = 0.1;
odom_.pose.covariance[35] = 0.05;
odom_.pose.covariance[14] = 1e6;
odom_.pose.covariance[21] = 1e6;
odom_.pose.covariance[28] = 1e6;
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.linear.z = 0;
odom_.twist.twist.angular.x = 0;
odom_.twist.twist.angular.y = 0;
odom_.twist.twist.angular.z = odom_vel_[2];
odom_pub_.publish(odom_); // publish odom message
if (publish_tf_)
{
odom_tf_.header = odom_.header;
odom_tf_.child_frame_id = odom_.child_frame_id;
odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
odom_tf_.transform.rotation = odom_.pose.pose.orientation;
tf_broadcaster_.sendTransform(odom_tf_);
}
/*
* Publish IMU data
*/
imu_msg_.header = joint_state_.header;
math::Quaternion quat = imu_->GetOrientation();
imu_msg_.orientation.x = quat.x;
imu_msg_.orientation.y = quat.y;
imu_msg_.orientation.z = quat.z;
imu_msg_.orientation.w = quat.w;
imu_msg_.orientation_covariance[0] = 1e6;
imu_msg_.orientation_covariance[4] = 1e6;
imu_msg_.orientation_covariance[8] = 0.05;
imu_msg_.angular_velocity.x = vel_angular_.x;
imu_msg_.angular_velocity.y = vel_angular_.y;
imu_msg_.angular_velocity.z = vel_angular_.z;
imu_msg_.angular_velocity_covariance[0] = 1e6;
imu_msg_.angular_velocity_covariance[4] = 1e6;
imu_msg_.angular_velocity_covariance[8] = 0.05;
math::Vector3 lin_acc = imu_->GetLinearAcceleration();
imu_msg_.linear_acceleration.x = lin_acc.x;
imu_msg_.linear_acceleration.y = lin_acc.y;
imu_msg_.linear_acceleration.z = lin_acc.z;
imu_pub_.publish(imu_msg_); // publish odom message
/*
* Propagate velocity commands
* TODO: Check how to simulate disabled motors, e.g. set MaxForce to zero, but then damping is important!
*/
if (((time_now - last_cmd_vel_time_).Double() > cmd_vel_timeout_) || !motors_enabled_)
{
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
}
joints_[LEFT]->SetVelocity(0, wheel_speed_cmd_[LEFT] / (wheel_diam_ / 2.0));
joints_[RIGHT]->SetVelocity(0, wheel_speed_cmd_[RIGHT] / (wheel_diam_ / 2.0));
joints_[LEFT]->SetMaxForce(0, torque_);
joints_[RIGHT]->SetMaxForce(0, torque_);
/*
* Cliff sensors
* Check each sensor separately
*/
// Left cliff sensor
if ((cliff_detected_left_ == false) &&
(cliff_sensor_left_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_left_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_left_ == true) &&
(cliff_sensor_left_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_left_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Centre cliff sensor
if ((cliff_detected_center_ == false) &&
(cliff_sensor_center_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_center_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_center_ == true) &&
(cliff_sensor_center_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_center_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Right cliff sensor
if ((cliff_detected_right_ == false) &&
(cliff_sensor_right_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_right_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_right_ == true) &&
(cliff_sensor_right_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_right_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
/*
* Bumpers
*/
// In order to simulate the three bumper sensors, a contact is assigned to one of the bumpers
// depending on its position. Each sensor covers a range of 60 degrees.
// +90 ... +30: left bumper
// +30 ... -30: centre bumper
// -30 ... -90: right bumper
// reset flags
bumper_left_is_pressed_ = false;
bumper_center_is_pressed_ = false;
bumper_right_is_pressed_ = false;
// parse contacts
msgs::Contacts contacts;
contacts = bumper_->GetContacts();
math::Pose current_pose = model_->GetWorldPose();
double robot_heading = current_pose.rot.GetYaw();
for (int i = 0; i < contacts.contact_size(); ++i)
{
double rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.pos.z;
if ((rel_contact_pos >= 0.015)
&& (rel_contact_pos <= 0.085)) // only consider contacts at the height of the bumper
{
// using the force normals below, since the contact position is given in world coordinates
// also negating the normal, because it points from contact to robot centre
double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
double relative_contact_angle = global_contact_angle - robot_heading;
if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
{
bumper_left_is_pressed_ = true;
}
else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
{
bumper_center_is_pressed_ = true;
}
else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
{
bumper_right_is_pressed_ = true;
}
}
}
// check for bumper state change
if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
}
void GazeboRosKobuki::spin()
{
while(ros::ok() && !shutdown_requested_)
{
ros::spinOnce();
}
}
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosKobuki);
} // namespace gazebo
Now you can compile the library
catkin_make gazebo_ros_kobuki
mv ~/ros_workspace/devel/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so
It's done.
Warning : If you want with one turtlebot, you need to change the launch file and add a prefix. Like with two robots.
It's work for me, but I really don't know if it's a good way to use multi robots on gazebo.
4 | No.4 Revision |
Hi,
We get same problem here and we don't know what we miss...
We tried with and without tf_prefix (some people say this is not compatible with Hydro).
In rviz we can't have the correct position of robot on map.
So the The question is :
number 1 :
How can we connected robot_name/odom -> robot_name/base_footprint on Hydro ?
The question 2 :
How can we connected /map -> /robot_name/odom or /odom -> /robot_name/base_footprint still on Hydro ?
Solution 1 :
I solve this problem with passing an argument to gmapping and then to use this arg as prefix for odom frame_id like this :
<arg name="robot_name" default="turtlebot" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="$(arg robot_name)/odom"/>
…
</node>
Now we get this tree :
map
| |
turtlebot_1/odom turtlebot_2/odom
and
odom
|
base_footprint
But now I can get the connection between /turtlebot_1/odom -> /turtlebot_1/base_footprint and respectively with turtlebot_2.
That's it I found !!
Question 1: So it's the gazebo plugin's of kobuki ros which it's does not take the namespace of robot.
robot.
To get two branches robot1/odom -> robot1/basefootprint and robot2/odom -> robot2/basefootprint we need to edit the plugin.
I don't know yet how I can tell to gazebo to use the modified plugin so I make a backup :
mv /opt/ros/hydro/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so.old
Then in my Ros workspace I clone the Git repot :
cd ros_workspace/src
~/ros_workspace/src
git clone h t t p s : //github.com/yujinrobot/kobuki_desktop.git
Don't have enouth karma to put a link.
Once you have all file you can edit the file in locate :
ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
~/ros_workspace/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki.cpp
For every publisher and subscriber I add
XXX.subscribe(node_name_ + "/XXXX");
Below all the edited file :
/**
* @author Marcus Liebhardt
*
* This work has been inspired by Nate Koenig's Gazebo plugin for the iRobot Create.
*/
#include <cmath>
#include <cstring>
#include <boost/bind.hpp>
#include <sensor_msgs/JointState.h>
#include <tf/LinearMath/Quaternion.h>
#include <gazebo/math/gzmath.hh>
#include "kobuki_gazebo_plugins/gazebo_ros_kobuki.h"
namespace gazebo
{
enum {LEFT= 0, RIGHT=1};
GazeboRosKobuki::GazeboRosKobuki() : shutdown_requested_(false)
{
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// Initialise variables
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
cliff_detected_left_ = false;
cliff_detected_center_ = false;
cliff_detected_right_ = false;
}
GazeboRosKobuki::~GazeboRosKobuki()
{
// rosnode_->shutdown();
shutdown_requested_ = true;
// Wait for spinner thread to end
// ros_spinner_thread_->join();
// delete spinner_thread_;
// delete rosnode_;
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_)
{
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->Get<std::string>("name");
gzdbg << "Plugin model name: " << model_name << "\n";
nh_ = ros::NodeHandle("");
// creating a private name pace until Gazebo implements topic remappings
nh_priv_ = ros::NodeHandle("/" + model_name);
node_name_ = model_name;
world_ = parent->GetWorld();
/*
* Prepare receiving motor power commands
*/
motor_power_sub_ = nh_priv_.subscribe(node_name_ + "/commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
motors_enabled_ = true;
/*
* Prepare joint state publishing
*/
if (sdf->HasElement("left_wheel_joint_name"))
{
left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("right_wheel_joint_name"))
{
right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
if (!joints_[LEFT] || !joints_[RIGHT])
{
ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
return;
}
joint_state_.header.frame_id = "Joint States";
joint_state_.name.push_back(left_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_.name.push_back(right_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(node_name_ + "/joint_states", 1);
/*
* Prepare publishing odometry data
*/
if (sdf->HasElement("publish_tf"))
{
publish_tf_ = sdf->GetElement("publish_tf")->Get<bool>();
if (publish_tf_)
{
ROS_INFO_STREAM("Will publish tf." << " [" << node_name_ <<"]");
}
else
{
ROS_INFO_STREAM("Won't publish tf." << " [" << node_name_ <<"]");
}
}
else
{
publish_tf_ = false;
ROS_INFO_STREAM("Couldn't find the 'publish tf' parameter in the model description."
<< " Won't publish tf." << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_separation"))
{
wheel_sep_ = sdf->GetElement("wheel_separation")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_diameter"))
{
wheel_diam_ = sdf->GetElement("wheel_diameter")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("torque"))
{
torque_ = sdf->GetElement("torque")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
odom_pub_ = nh_.advertise<nav_msgs::Odometry>(node_name_ + "/odom", 1);
odom_reset_sub_ = nh_priv_.subscribe(node_name_ + "/commands/reset_odometry", 10, &GazeboRosKobuki::resetOdomCB, this);
/*
* Prepare receiving velocity commands
*/
if (sdf->HasElement("velocity_command_timeout"))
{
cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
last_cmd_vel_time_ = world_->GetSimTime();
cmd_vel_sub_ = nh_priv_.subscribe(node_name_ + "/commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);
/*
* Prepare cliff sensors
*/
std::string cliff_sensor_left_name, cliff_sensor_center_name, cliff_sensor_right_name;
if (sdf->HasElement("cliff_sensor_left_name"))
{
cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_center_name"))
{
cliff_sensor_center_name = sdf->GetElement("cliff_sensor_center_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_right_name"))
{
cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
cliff_sensor_center_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name));
cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
if (!cliff_sensor_left_)
{
ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_center_)
{
ROS_ERROR_STREAM("Couldn't find the center cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_right_)
{
ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_detection_threshold"))
{
cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->Get<double>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_->SetActive(true);
cliff_sensor_center_->SetActive(true);
cliff_sensor_right_->SetActive(true);
cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>(node_name_ + "/events/cliff", 1);
/*
* Prepare bumper
*/
std::string bumper_name;
if (sdf->HasElement("bumper_name"))
{
bumper_name = sdf->GetElement("bumper_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
sensors::SensorManager::Instance()->GetSensor(bumper_name));
if (!bumper_)
{
ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
return;
}
bumper_->SetActive(true);
bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>(node_name_ + "/events/bumper", 1);
/*
* Prepare IMU
*/
std::string imu_name;
if (sdf->HasElement("imu_name"))
{
imu_name = sdf->GetElement("imu_name")->Get<std::string>();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of IMU sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
imu_ = boost::shared_dynamic_cast<sensors::ImuSensor>(
sensors::SensorManager::Instance()->GetSensor(imu_name));
if (!imu_)
{
ROS_ERROR_STREAM("Couldn't find the IMU in the model! [" << node_name_ <<"]");
return;
}
imu_->SetActive(true);
imu_pub_ = nh_priv_.advertise<sensor_msgs::Imu>(node_name_ + "/sensors/imu_data", 1);
prev_update_time_ = world_->GetSimTime();
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
void GazeboRosKobuki::motorPowerCB(const kobuki_msgs::MotorPowerPtr &msg)
{
if ((msg->state == kobuki_msgs::MotorPower::ON) && (!motors_enabled_))
{
motors_enabled_ = true;
ROS_INFO_STREAM("Motors fired up. [" << node_name_ << "]");
}
else if ((msg->state == kobuki_msgs::MotorPower::OFF) && (motors_enabled_))
{
motors_enabled_ = false;
ROS_INFO_STREAM("Motors taking a rest. [" << node_name_ << "]");
}
}
void GazeboRosKobuki::cmdVelCB(const geometry_msgs::TwistConstPtr &msg)
{
last_cmd_vel_time_ = world_->GetSimTime();
wheel_speed_cmd_[LEFT] = msg->linear.x - msg->angular.z * (wheel_sep_) / 2;
wheel_speed_cmd_[RIGHT] = msg->linear.x + msg->angular.z * (wheel_sep_) / 2;
}
void GazeboRosKobuki::resetOdomCB(const std_msgs::EmptyConstPtr &msg)
{
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
void GazeboRosKobuki::OnUpdate()
{
/*
* First process ROS callbacks
*/
ros::spinOnce();
/*
* Update current time and time step
*/
common::Time time_now = world_->GetSimTime();
common::Time step_time = time_now - prev_update_time_;
prev_update_time_ = time_now;
/*
* Joint states
*/
joint_state_.header.stamp = ros::Time::now();
joint_state_.header.frame_id = node_name_ + "/base_link";
joint_state_.position[LEFT] = joints_[LEFT]->GetAngle(0).Radian();
joint_state_.velocity[LEFT] = joints_[LEFT]->GetVelocity(0);
joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
joint_state_.velocity[RIGHT] = joints_[RIGHT]->GetVelocity(0);
joint_state_pub_.publish(joint_state_);
/*
* Odometry (encoders & IMU)
*/
odom_.header.stamp = joint_state_.header.stamp;
odom_.header.frame_id = node_name_ + "/odom";
odom_.child_frame_id = node_name_ + "/base_footprint";
// Distance travelled by main wheels
double d1, d2;
double dr, da;
d1 = d2 = 0;
dr = da = 0;
d1 = step_time.Double() * (wheel_diam_ / 2) * joints_[LEFT]->GetVelocity(0);
d2 = step_time.Double() * (wheel_diam_ / 2) * joints_[RIGHT]->GetVelocity(0);
// Can see NaN values here, just zero them out if needed
if (isnan(d1))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d1. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[LEFT]->GetVelocity(0));
d1 = 0;
}
if (isnan(d2))
{
ROS_WARN_STREAM_THROTTLE(0.1, "Gazebo ROS Kobuki plugin: NaN in d2. Step time: " << step_time.Double()
<< ", WD: " << wheel_diam_ << ", velocity: " << joints_[RIGHT]->GetVelocity(0));
d2 = 0;
}
dr = (d1 + d2) / 2;
da = (d2 - d1) / wheel_sep_; // ignored
// Just as in the Kobuki driver, the angular velocity is taken directly from the IMU
vel_angular_ = imu_->GetAngularVelocity();
// Compute odometric pose
odom_pose_[0] += dr * cos( odom_pose_[2] );
odom_pose_[1] += dr * sin( odom_pose_[2] );
odom_pose_[2] += vel_angular_.z * step_time.Double();
// Compute odometric instantaneous velocity
odom_vel_[0] = dr / step_time.Double();
odom_vel_[1] = 0.0;
odom_vel_[2] = vel_angular_.z;
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
tf::Quaternion qt;
qt.setEuler(0,0,odom_pose_[2]);
odom_.pose.pose.orientation.x = qt.getX();
odom_.pose.pose.orientation.y = qt.getY();
odom_.pose.pose.orientation.z = qt.getZ();
odom_.pose.pose.orientation.w = qt.getW();
odom_.pose.covariance[0] = 0.1;
odom_.pose.covariance[7] = 0.1;
odom_.pose.covariance[35] = 0.05;
odom_.pose.covariance[14] = 1e6;
odom_.pose.covariance[21] = 1e6;
odom_.pose.covariance[28] = 1e6;
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.linear.z = 0;
odom_.twist.twist.angular.x = 0;
odom_.twist.twist.angular.y = 0;
odom_.twist.twist.angular.z = odom_vel_[2];
odom_pub_.publish(odom_); // publish odom message
if (publish_tf_)
{
odom_tf_.header = odom_.header;
odom_tf_.child_frame_id = odom_.child_frame_id;
odom_tf_.transform.translation.x = odom_.pose.pose.position.x;
odom_tf_.transform.translation.y = odom_.pose.pose.position.y;
odom_tf_.transform.translation.z = odom_.pose.pose.position.z;
odom_tf_.transform.rotation = odom_.pose.pose.orientation;
tf_broadcaster_.sendTransform(odom_tf_);
}
/*
* Publish IMU data
*/
imu_msg_.header = joint_state_.header;
math::Quaternion quat = imu_->GetOrientation();
imu_msg_.orientation.x = quat.x;
imu_msg_.orientation.y = quat.y;
imu_msg_.orientation.z = quat.z;
imu_msg_.orientation.w = quat.w;
imu_msg_.orientation_covariance[0] = 1e6;
imu_msg_.orientation_covariance[4] = 1e6;
imu_msg_.orientation_covariance[8] = 0.05;
imu_msg_.angular_velocity.x = vel_angular_.x;
imu_msg_.angular_velocity.y = vel_angular_.y;
imu_msg_.angular_velocity.z = vel_angular_.z;
imu_msg_.angular_velocity_covariance[0] = 1e6;
imu_msg_.angular_velocity_covariance[4] = 1e6;
imu_msg_.angular_velocity_covariance[8] = 0.05;
math::Vector3 lin_acc = imu_->GetLinearAcceleration();
imu_msg_.linear_acceleration.x = lin_acc.x;
imu_msg_.linear_acceleration.y = lin_acc.y;
imu_msg_.linear_acceleration.z = lin_acc.z;
imu_pub_.publish(imu_msg_); // publish odom message
/*
* Propagate velocity commands
* TODO: Check how to simulate disabled motors, e.g. set MaxForce to zero, but then damping is important!
*/
if (((time_now - last_cmd_vel_time_).Double() > cmd_vel_timeout_) || !motors_enabled_)
{
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
}
joints_[LEFT]->SetVelocity(0, wheel_speed_cmd_[LEFT] / (wheel_diam_ / 2.0));
joints_[RIGHT]->SetVelocity(0, wheel_speed_cmd_[RIGHT] / (wheel_diam_ / 2.0));
joints_[LEFT]->SetMaxForce(0, torque_);
joints_[RIGHT]->SetMaxForce(0, torque_);
/*
* Cliff sensors
* Check each sensor separately
*/
// Left cliff sensor
if ((cliff_detected_left_ == false) &&
(cliff_sensor_left_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_left_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_left_ == true) &&
(cliff_sensor_left_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_left_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::LEFT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_left_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Centre cliff sensor
if ((cliff_detected_center_ == false) &&
(cliff_sensor_center_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_center_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_center_ == true) &&
(cliff_sensor_center_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_center_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::CENTER;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_center_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
// Right cliff sensor
if ((cliff_detected_right_ == false) &&
(cliff_sensor_right_->GetRange(0) >= cliff_detection_threshold_))
{
cliff_detected_right_ = true;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::CLIFF;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
else if ((cliff_detected_right_ == true) &&
(cliff_sensor_right_->GetRange(0) < cliff_detection_threshold_))
{
cliff_detected_right_ = false;
cliff_event_.sensor = kobuki_msgs::CliffEvent::RIGHT;
cliff_event_.state = kobuki_msgs::CliffEvent::FLOOR;
// convert distance back to an AD reading
cliff_event_.bottom = (int)(76123.0f * atan2(0.995f, cliff_sensor_right_->GetRange(0)));
cliff_event_pub_.publish(cliff_event_);
}
/*
* Bumpers
*/
// In order to simulate the three bumper sensors, a contact is assigned to one of the bumpers
// depending on its position. Each sensor covers a range of 60 degrees.
// +90 ... +30: left bumper
// +30 ... -30: centre bumper
// -30 ... -90: right bumper
// reset flags
bumper_left_is_pressed_ = false;
bumper_center_is_pressed_ = false;
bumper_right_is_pressed_ = false;
// parse contacts
msgs::Contacts contacts;
contacts = bumper_->GetContacts();
math::Pose current_pose = model_->GetWorldPose();
double robot_heading = current_pose.rot.GetYaw();
for (int i = 0; i < contacts.contact_size(); ++i)
{
double rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.pos.z;
if ((rel_contact_pos >= 0.015)
&& (rel_contact_pos <= 0.085)) // only consider contacts at the height of the bumper
{
// using the force normals below, since the contact position is given in world coordinates
// also negating the normal, because it points from contact to robot centre
double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
double relative_contact_angle = global_contact_angle - robot_heading;
if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
{
bumper_left_is_pressed_ = true;
}
else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
{
bumper_center_is_pressed_ = true;
}
else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
{
bumper_right_is_pressed_ = true;
}
}
}
// check for bumper state change
if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
}
void GazeboRosKobuki::spin()
{
while(ros::ok() && !shutdown_requested_)
{
ros::spinOnce();
}
}
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosKobuki);
} // namespace gazebo
Now you can compile the library
catkin_make gazebo_ros_kobuki
mv ~/ros_workspace/devel/lib/libgazebo_ros_kobuki.so /opt/ros/hydro/lib/libgazebo_ros_kobuki.so
It's done.
Warning : : If you want to use with one turtlebot, you need to change the launch file and add a prefix. Like with two robots.
It's work for me, but I really don't know if it's a good way to use multi robots on gazebo. If you run rqt_graph you can see that gazebo publish into robot_name/joint_states and robot_name/odom. It's a good news for use.
Question 2 : To get the gmapping publish the map on correct topics and correct frame we need to specified the odom_frame and base_frame of robot.
Then we need to modify the output_frame_id of the lazer scan to become robot_name/camera_depth_frame otherwise gmapping push information into camera_depth_frame which is connected to nothing.
On Rviz all robots are in same place another stuff to do... but we can see the map of robot1 and robot2 !
Here is the launch files I use to put many robot in Gazebo and to view it with Rviz correctly.
Environment :
<launch>
<!-- Gazebo config -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="false"/>
<arg name="gui" value="false"/>
<arg name="world_name" value="$(find exploration)/worlds/gazebo_z.world"/>
<!--
<arg name="world_name" value="worlds/willowgarage.world"/>
-->
</include>
<!-- Launch for multi robots -->
<include file="$(find exploration)/config/robot/robots.launch.xml" />
</launch>
Now the launch for multi-robots :
<launch>
<!-- Plusieurs robots -->
<!-- Turtlebot configuration pour tout les robots -->
<arg name="base" default="$(optenv TURTLEBOT_BASE kobuki)"/>
<arg name="battery" default="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>
<arg name="stacks" default="$(optenv TURTLEBOT_STACKS hexagons)"/>
<arg name="3d_sensor" default="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>
<arg name="model" default="$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro" />
<!-- send the robot XML to param server -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />
<!-- Bring up robot1 -->
<group ns="turtlebot_1">
<include file="$(find exploration)/config/robot/robot.launch.xml" >
<arg name="robot_name" value="turtlebot_1" />
</include>
</group>
<group ns="turtlebot_2">
<include file="$(find exploration)/config/robot/robot.launch.xml" >
<arg name="x" value="1.0" />
<arg name="robot_name" value="turtlebot_2" />
</include>
</group>
</launch>
Robot launch
<launch>
<!-- Un robot -->
<arg name="x" default="0.0"/>
<arg name="y" default="0.0"/>
<arg name="z" default="0.0"/>
<arg name="robot_name" default="turtlebot" />
<!-- Gazebo model spawner for turtlebot -->
<node name="spawn_$(arg robot_name)" pkg="gazebo_ros" type="spawn_model"
args="-param /robot_description
-urdf
-unpause
-x $(arg x)
-y $(arg y)
-z $(arg z)
-model $(arg robot_name)
" respawn="false" output="screen" >
</node>
<!-- Modification du mobile_base en robot_name pour correspondre avec notre robot -->
<node pkg="nodelet" type="nodelet" name="$(arg robot_name)_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg robot_name)_nodelet_manager">
<param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
<remap from="cmd_vel_mux/output" to="commands/velocity"/>
</node>
<!-- bumper2pc.launch.xml -->
<node pkg="nodelet" type="nodelet" name="bumper2pointcloud" args="load kobuki_bumper2pc/Bumper2PcNodelet $(arg robot_name)_nodelet_manager">
<param name="pointcloud_radius" value="0.24"/>
<remap from="bumper2pointcloud/pointcloud" to="sensors/bumper_pointcloud"/>
<remap from="bumper2pointcloud/core_sensors" to="sensors/core"/>
</node>
<!-- End bumper2pc.launch.xml -->
<!-- robot_state_publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
<param name="tf_prefix" type="string" value="$(arg robot_name)" />
</node>
<!-- fake lazer -->
<node pkg="nodelet" type="nodelet" name="$(arg robot_name)_laserscan_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet $(arg robot_name)_laserscan_nodelet_manager">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="$(arg robot_name)/camera_depth_frame"/>
<param name="range_min" value="0.45"/>
<remap from="image" to="camera/depth/image_raw"/>
<remap from="scan" to="scan"/>
<!-- Velocity smoother -->
<node pkg="nodelet" type="nodelet" name="$(arg robot_name)_navigation_velocity_smoother" args="load yocs_velocity_smoother/VelocitySmootherNodelet \
$(arg robot_name)_nodelet_manager">
<rosparam file="$(find turtlebot_bringup)/param/defaults/smoother.yaml" command="load"/>
<remap from="$(arg robot_name)_navigation_velocity_smoother/smooth_cmd_vel" to="cmd_vel_mux/input/navi"/>
<remap from="$(arg robot_name)_navigation_velocity_smoother/odometry" to="odom"/>
<remap from="$(arg robot_name)_navigation_velocity_smoother/robot_cmd_vel" to="commands/velocity"/>
</node>
<!-- End Velocity smoother -->
<!-- Safety controller-->
<node pkg="nodelet" type="nodelet" name="$(arg robot_name)_kobuki_safety_controller" args="load kobuki_safety_controller/SafetyControllerNodelet $(\
arg robot_name)_nodelet_manager">
<remap from="$(arg robot_name)_kobuki_safety_controller/cmd_vel" to="cmd_vel_mux/input/safety_controller"/>
<remap from="$(arg robot_name)_kobuki_safety_controller/events/bumper" to="events/bumper"/>
<remap from="$(arg robot_name)_kobuki_safety_controller/events/cliff" to="events/cliff"/>
<remap from="$(arg robot_name)_kobuki_safety_controller/events/wheel_drop" to="events/wheel_drop"/>
</node>
<!-- End Safety controller -->
<!-- Gmapping -->
<include file="$(find exploration)/config/gmapping_gazebo/gmapping.launch" >
<arg name="scan_topic" value="scan" />
<arg name="robot_name" value="$(arg robot_name)" />
</include>
<!--
<include file="$(find exploration)/config/move_base_gazebo/move_base.xml" />
-->
</launch>
And gmapping :
<launch>
<arg name="scan_topic" default="scan" />
<arg name="robot_name" default="turtlebot" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="odom_frame" value="$(arg robot_name)/odom"/>
<param name="base_frame" value="$(arg robot_name)/base_footprint"/>
<param name="map_frame" value="/map" />
<param name="output_frame_id" value="$(arg robot_name)/camera_depth_frame"/>
<param name="map_update_interval" value="5.0"/>
<param name="maxUrange" value="3.2"/>
<param name="maxRange" value="20.0" />
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="10.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<param name="xmin" value="-5.0"/>
<param name="ymin" value="-5.0"/>
<param name="xmax" value="5.0"/>
<param name="ymax" value="5.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
<!-- from our old setup:
<param name="transform_publish_period" value="0.05" />
-->
</node>
</launch>