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

Fixes for compilation and warnings in Lunar-devel #573

Merged
merged 24 commits into from Apr 28, 2017

Conversation

j-rivero
Copy link
Contributor

Initial work to solve compilation and deprecation warnings on Lunar-devel.

@j-rivero
Copy link
Contributor Author

@osrf-jenkins run test please

@@ -236,8 +236,8 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
auto maxAngle = this->parent_ray_sensor_->AngleMax();
auto minAngle = this->parent_ray_sensor_->AngleMin();
#else
math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
ignition::math::Angle maxAngle = this->parent_ray_sensor_->PositionMax();
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is wrong I think since GAZEBO_MAJOR_VERSION <6 don't use inition lib, no?

@@ -157,7 +157,7 @@ void GazeboRosBumper::OnContact()
*Simulator::Instance()->GetMRMutex());
// look through all models in the world, search for body
// name that matches frameName
phyaics::Model_V all_models = World::Instance()->GetModels();
phyaics::Model_V all_models = World::Instance()->Models();
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

typo even if not used : physics

@@ -355,7 +355,7 @@ void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov)
#if GAZEBO_MAJOR_VERSION >= 7
this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
#else
this->camera_->SetHFOV(gazebo::math::Angle(hfov->data));
this->camera_->SetHFOV(ignition::math::Angle(hfov->data));
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this looks also wrong

@@ -295,8 +295,8 @@ bool GazeboRosJointPoseTrajectory::SetTrajectory(
this->disable_physics_updates_ = req.disable_physics_updates;
if (this->disable_physics_updates_)
{
this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
this->world_->EnablePhysicsEngine(false);
this->physics_engine_enabled_ = this->world_->SetPhysicsEnabled();
Copy link

@khancyr khancyr Apr 27, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

either do as before : + this->world_->SetPhysicsEnabled(false);

  • this->physics_engine_enabled_ = false;

Or this->world_->SetPhysicsEnabled(false)
this->physics_engine_enabled_ = this->world_->PhysicsEnabled();

@@ -297,8 +297,8 @@ bool GazeboRosJointTrajectory::SetTrajectory(
this->disable_physics_updates_ = req.disable_physics_updates;
if (this->disable_physics_updates_)
{
this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine();
this->world_->EnablePhysicsEngine(false);
this->physics_engine_enabled_ = this->world_->SetPhysicsEnabled();
Copy link

@khancyr khancyr Apr 27, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

either do as before : + this->world_->SetPhysicsEnabled(false);

this->physics_engine_enabled_ = false;
Or
this->world_->SetPhysicsEnabled(false)
this->physics_engine_enabled_ = this->world_->PhysicsEnabled();

@@ -312,7 +311,7 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe
#if GAZEBO_MAJOR_VERSION >= 4
joint_steering_->SetPosition(0, applied_angle);
#else
joint_steering_->SetAngle(0, math::Angle(applied_angle));
joint_steering_->SetAngle(0,ignition::math::Angle(applied_angle));
Copy link

@khancyr khancyr Apr 27, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

doesn't look good

gazebo::math::Pose frame_pose = frame->GetWorldPose();
initial_xyz = frame_pose.rot.RotateVector(initial_xyz);
ignition::math::Pose3d frame_pose = frame->WorldPose();
initial_xyz = frame_pose.Rot().RotateVector(initial_xyz);
initial_xyz += frame_pose.pos;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

miss initial_xyz += frame_pose.Pos();
initial_q *= frame_pose.Rot();

gazebo::math::Vector3 model_pos = model_pose.pos;
gazebo::math::Quaternion model_rot = model_pose.rot;
ignition::math::Pose3d model_pose = model->WorldPose();
ignition::math::Vector3d model_pos = model_pose.pos;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ignition::math::Vector3d    model_pos = model_pose.Pos();
ignition::math::Quaterniond model_rot = model_pose.Rot();



if (frame)
{
// convert to relative pose
gazebo::math::Pose frame_pose = frame->GetWorldPose();
ignition::math::Pose3d frame_pose = frame->WorldPose();
model_pos = model_pos - frame_pose.pos;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pos()

model_pos = model_pos - frame_pose.pos;
model_pos = frame_pose.rot.RotateVectorReverse(model_pos);
model_rot *= frame_pose.rot.GetInverse();
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

model_rot *= frame_pose.Rot().Inverse();

ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame
ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame
model_linear_vel = frame_pose.Rot().RotateVector(model_linear_vel - frame_vpos);
model_angular_vel = frame_pose.Rot().RotateVector(model_angular_vel - frame_veul);
}
/// @todo: FIXME map is really wrong, need to use tf here somehow
else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map")
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

// fill in response
res.pose.position.x = model_pos.X();
res.pose.position.y = model_pos.Y();
res.pose.position.z = model_pos.Z();
res.pose.orientation.w = model_rot.W();
res.pose.orientation.x = model_rot.X();
res.pose.orientation.y = model_rot.Y();
res.pose.orientation.z = model_rot.Z();

res.twist.linear.x = model_linear_vel.X();
res.twist.linear.y = model_linear_vel.Y();
res.twist.linear.z = model_linear_vel.Z();
res.twist.angular.x = model_angular_vel.X();
res.twist.angular.y = model_angular_vel.Y();
res.twist.angular.z = model_angular_vel.Z();

@khancyr
Copy link

khancyr commented Apr 27, 2017

Hello, I spot plenties of changes miss ! I will rebase my gazebo9 branch and submit you a PR, it will be simplier than trying to correct all on github interface ... most are in gazebo_ros_api_plugin.cpp and gazebo_ros_control

* Whitespace fixes

* revert wrong changes

* correct typo in launch file

* update missing file
@j-rivero
Copy link
Contributor Author

Hello, I spot plenties of changes miss ! I will rebase my gazebo9 branch and submit you a PR, it will be simplier than trying to correct all on github interface ... most are in gazebo_ros_api_plugin.cpp and gazebo_ros_control

Thanks for that. The fixes have been merged. I expect the builds to fail because we still miss some gazebo8 packages in the ROS repo.

@j-rivero j-rivero mentioned this pull request Apr 27, 2017
@j-rivero
Copy link
Contributor Author

First clean build. Here we go.

@j-rivero j-rivero merged commit 675f764 into lunar-devel Apr 28, 2017
@scpeters scpeters deleted the lunar-devel-initial-fix branch November 28, 2017 18:11
cohen39 pushed a commit to cohen39/gazebo_ros_pkgs that referenced this pull request Nov 15, 2021
Multiple fixes for compilation and warnings coming from Gazebo8 and ignition-math3
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

2 participants