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

Updated gazebo_ros_pkg to latest gazebo api (gazebo 7.0.1). #375

Closed
Show file tree
Hide file tree
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
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,9 @@
#define JOINT_STATE_PUBLISHER_PLUGIN_HH

#include <boost/bind.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
Expand Down
8 changes: 4 additions & 4 deletions gazebo_plugins/src/gazebo_ros_block_laser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -230,8 +230,8 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)

this->parent_ray_sensor_->SetActive(false);

math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
math::Angle maxAngle = this->parent_ray_sensor_->AngleMax();
math::Angle minAngle = this->parent_ray_sensor_->AngleMin();

double maxRange = this->parent_ray_sensor_->GetRangeMax();
double minRange = this->parent_ray_sensor_->GetRangeMin();
Expand All @@ -240,8 +240,8 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)

int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax();
math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin();
math::Angle verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
math::Angle verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();

double yDiff = maxAngle.Radian() - minAngle.Radian();
double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
Expand Down
12 changes: 6 additions & 6 deletions gazebo_plugins/src/gazebo_ros_diff_drive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,8 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf
joints_.resize ( 2 );
joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" );
joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" );
joints_[LEFT]->SetMaxForce ( 0, wheel_torque );
joints_[RIGHT]->SetMaxForce ( 0, wheel_torque );
joints_[LEFT]->SetParam("fmax", 0, wheel_torque );
joints_[RIGHT]->SetParam("fmax", 0, wheel_torque );



Expand Down Expand Up @@ -170,8 +170,8 @@ void GazeboRosDiffDrive::Reset()
pose_encoder_.theta = 0;
x_ = 0;
rot_ = 0;
joints_[LEFT]->SetMaxForce ( 0, wheel_torque );
joints_[RIGHT]->SetMaxForce ( 0, wheel_torque );
joints_[LEFT]->SetParam("fmax", 0, wheel_torque );
joints_[RIGHT]->SetParam("fmax", 0, wheel_torque );
}

void GazeboRosDiffDrive::publishWheelJointState()
Expand Down Expand Up @@ -221,8 +221,8 @@ void GazeboRosDiffDrive::UpdateChild()
(this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e)
*/
for ( int i = 0; i < 2; i++ ) {
if ( fabs(wheel_torque -joints_[i]->GetMaxForce ( 0 )) > 1e-6 ) {
joints_[i]->SetMaxForce ( 0, wheel_torque );
if ( fabs(wheel_torque -joints_[i]->GetParam("fmax", 0 )) > 1e-6 ) {
joints_[i]->SetParam("fmax", 0, wheel_torque );
}
}

Expand Down
8 changes: 4 additions & 4 deletions gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -256,10 +256,10 @@ namespace gazebo {
gzthrow(error);
}

joints[LEFT_FRONT]->SetMaxForce(0, torque);
joints[RIGHT_FRONT]->SetMaxForce(0, torque);
joints[LEFT_REAR]->SetMaxForce(0, torque);
joints[RIGHT_REAR]->SetMaxForce(0, torque);
joints[LEFT_FRONT]->SetParam("fmax", 0, torque);
joints[RIGHT_FRONT]->SetParam("fmax", 0, torque);
joints[LEFT_REAR]->SetParam("fmax", 0, torque);
joints[RIGHT_REAR]->SetParam("fmax", 0, torque);

// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
Expand Down
4 changes: 2 additions & 2 deletions gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,8 +108,8 @@ void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _
odomOptions["world"] = WORLD;
gazebo_ros_->getParameter<OdomSource> ( odom_source_, "odometrySource", odomOptions, WORLD );

joint_wheel_actuated_->SetMaxForce ( 0, wheel_torque_ );
joint_steering_->SetMaxForce ( 0, wheel_torque_ );
joint_wheel_actuated_->SetParam("fmax", 0, wheel_torque_ );
joint_steering_->SetParam("fmax", 0, wheel_torque_ );


// Initialize update rate stuff
Expand Down
2 changes: 1 addition & 1 deletion gazebo_ros_control/src/default_robot_hw_sim.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,7 @@ bool DefaultRobotHWSim::initSim(
// joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are
// going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is
// going to be called.
joint->SetMaxForce(0, joint_effort_limits_[j]);
joint->SetParam("fmax", 0, joint_effort_limits_[j]);
}
}
}
Expand Down