diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp index 045d6b7..e368453 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp @@ -309,6 +309,8 @@ void GazeboRosKobuki::setupRosApi(std::string& model_name) { std::string base_prefix; gazebo_ros_->node()->param("base_prefix", base_prefix, std::string("mobile_base")); + if (base_prefix.length() > 0) + base_prefix += "/"; // Public topics @@ -325,32 +327,32 @@ void GazeboRosKobuki::setupRosApi(std::string& model_name) // Private Topics // motor power - std::string motor_power_topic = base_prefix + "/commands/motor_power"; + std::string motor_power_topic = base_prefix + "commands/motor_power"; motor_power_sub_ = gazebo_ros_->node()->subscribe(motor_power_topic, 10, &GazeboRosKobuki::motorPowerCB, this); ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), motor_power_topic.c_str()); - std::string odom_reset_topic = base_prefix + "/commands/reset_odometry"; + std::string odom_reset_topic = base_prefix + "commands/reset_odometry"; odom_reset_sub_ = gazebo_ros_->node()->subscribe(odom_reset_topic, 10, &GazeboRosKobuki::resetOdomCB, this); ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), odom_reset_topic.c_str()); // cmd_vel - std::string cmd_vel = base_prefix + "/commands/velocity"; + std::string cmd_vel = base_prefix + "commands/velocity"; cmd_vel_sub_ = gazebo_ros_->node()->subscribe(cmd_vel, 100, &GazeboRosKobuki::cmdVelCB, this); ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), motor_power_topic.c_str()); // cliff - std::string cliff_topic = base_prefix + "/events/cliff"; + std::string cliff_topic = base_prefix + "events/cliff"; cliff_event_pub_ = gazebo_ros_->node()->advertise(cliff_topic, 1); ROS_INFO("%s: Advertise Cliff[%s]!", gazebo_ros_->info(), cliff_topic.c_str()); // bumper - std::string bumper_topic = base_prefix + "/events/bumper"; + std::string bumper_topic = base_prefix + "events/bumper"; bumper_event_pub_ = gazebo_ros_->node()->advertise(bumper_topic, 1); ROS_INFO("%s: Advertise Bumper[%s]!", gazebo_ros_->info(), bumper_topic.c_str()); // IMU - std::string imu_topic = base_prefix + "/sensors/imu_data"; + std::string imu_topic = base_prefix + "sensors/imu_data"; imu_pub_ = gazebo_ros_->node()->advertise(imu_topic, 1); ROS_INFO("%s: Advertise IMU[%s]!", gazebo_ros_->info(), imu_topic.c_str()); }