From 05dbcae34cc4811949000053585f35b3ff8a356d Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 13:42:15 -0700 Subject: [PATCH 1/6] using c++11 for gazebo as gazebo7 headers are c+11 --- kobuki_gazebo_plugins/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/kobuki_gazebo_plugins/CMakeLists.txt b/kobuki_gazebo_plugins/CMakeLists.txt index c3917f2..b4bf0d0 100644 --- a/kobuki_gazebo_plugins/CMakeLists.txt +++ b/kobuki_gazebo_plugins/CMakeLists.txt @@ -3,6 +3,7 @@ project(kobuki_gazebo_plugins) find_package(gazebo REQUIRED) + find_package(catkin REQUIRED COMPONENTS gazebo_ros gazebo_plugins geometry_msgs @@ -25,6 +26,8 @@ catkin_package(INCLUDE_DIRS include std_msgs tf) +set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} " -std=c++11") + link_directories(${GAZEBO_LIBRARY_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} From bfce630dfb61871926900d6b749edf30d511c962 Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 13:54:12 -0700 Subject: [PATCH 2/6] Use std pointer cast instead of boost Gazebo 7 uses std::shared_ptr so the cast needs to be from std not boost --- kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp index 045d6b7..3b6948b 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp @@ -212,11 +212,11 @@ bool GazeboRosKobuki::prepareCliffSensor() << " Did you specify it?" << " [" << node_name_ <<"]"); return false; } - cliff_sensor_left_ = boost::dynamic_pointer_cast( + cliff_sensor_left_ = std::dynamic_pointer_cast( sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name)); - cliff_sensor_center_ = boost::dynamic_pointer_cast( + cliff_sensor_center_ = std::dynamic_pointer_cast( sensors::SensorManager::Instance()->GetSensor(cliff_sensor_center_name)); - cliff_sensor_right_ = boost::dynamic_pointer_cast( + cliff_sensor_right_ = std::dynamic_pointer_cast( sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name)); if (!cliff_sensor_left_) { @@ -267,7 +267,7 @@ bool GazeboRosKobuki::prepareBumper() << " Did you specify it?" << " [" << node_name_ <<"]"); return false; } - bumper_ = boost::dynamic_pointer_cast( + bumper_ = std::dynamic_pointer_cast( sensors::SensorManager::Instance()->GetSensor(bumper_name)); if (!bumper_) { @@ -294,7 +294,7 @@ bool GazeboRosKobuki::prepareIMU() << " Did you specify it?" << " [" << node_name_ <<"]"); return false; } - imu_ = boost::dynamic_pointer_cast( + imu_ = std::dynamic_pointer_cast( sensors::get_sensor(world_->GetName()+"::"+node_name_+"::base_footprint::"+imu_name)); if (!imu_) { From 82edbdcf99d0aec20ad03a40a0e4dfae5af9417e Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 13:56:13 -0700 Subject: [PATCH 3/6] use isnan form std --- kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp index 81340fc..edc9328 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp @@ -73,13 +73,13 @@ void GazeboRosKobuki::updateOdometry(common::Time& step_time) 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)) + if (std::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)) + if (std::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)); From 2f553db981c979ca1c4cc71b9022c29763bc180e Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 13:57:41 -0700 Subject: [PATCH 4/6] changed setMaxForce(deprecated) to setparam fmax --- kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp index edc9328..312d090 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp @@ -178,8 +178,8 @@ void GazeboRosKobuki::propagateVelocityCommands() } 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_); + joints_[LEFT]->SetParam("fmax", 0, torque_); + joints_[RIGHT]->SetParam("fmax", 0, torque_); } /* From eca12ebb138cac9721e58b1627c68a19f981a322 Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 14:10:28 -0700 Subject: [PATCH 5/6] gazebo7 removes 'Get' from IMU data elements --- kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp index 312d090..d74672e 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp @@ -89,7 +89,7 @@ void GazeboRosKobuki::updateOdometry(common::Time& step_time) 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(); + vel_angular_ = imu_->AngularVelocity(); // Compute odometric pose odom_pose_[0] += dr * cos( odom_pose_[2] ); @@ -144,7 +144,7 @@ void GazeboRosKobuki::updateOdometry(common::Time& step_time) void GazeboRosKobuki::updateIMU() { imu_msg_.header = joint_state_.header; - math::Quaternion quat = imu_->GetOrientation(); + math::Quaternion quat = imu_->Orientation(); imu_msg_.orientation.x = quat.x; imu_msg_.orientation.y = quat.y; imu_msg_.orientation.z = quat.z; @@ -158,7 +158,7 @@ void GazeboRosKobuki::updateIMU() 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(); + math::Vector3 lin_acc = imu_->LinearAcceleration(); imu_msg_.linear_acceleration.x = lin_acc.x; imu_msg_.linear_acceleration.y = lin_acc.y; imu_msg_.linear_acceleration.z = lin_acc.z; From 410e4da63959026872f4b422c053299b1c83c50f Mon Sep 17 00:00:00 2001 From: Rohan Agrawal Date: Fri, 8 Jul 2016 18:06:11 -0700 Subject: [PATCH 6/6] Remove fmax, as robot doesn't move with it --- kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp index d74672e..103853c 100644 --- a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp +++ b/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp @@ -178,8 +178,6 @@ void GazeboRosKobuki::propagateVelocityCommands() } 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]->SetParam("fmax", 0, torque_); - joints_[RIGHT]->SetParam("fmax", 0, torque_); } /*