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

Citadel support #4

Merged
merged 4 commits into from
Jun 25, 2021
Merged
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
2 changes: 1 addition & 1 deletion .github/workflows/ci.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ jobs:
rosdep init
rosdep update
rosdep install --from-paths ./ -i -y --rosdistro foxy \
--ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1"
--ignore-src --skip-keys "ignition-gazebo5"
- name: Build project
id: build
run: |
Expand Down
2 changes: 1 addition & 1 deletion Dockerfile/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ RUN mkdir -p /home/ros2_ws/src \
&& git clone https://github.com/ignitionrobotics/ign_ros2_control/ -b ahcorde/ign_ros2_control \
&& rosdep update \
&& rosdep install --from-paths ./ -i -y --rosdistro foxy \
--ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1"
--ignore-src --skip-keys "ignition-gazebo5"

RUN cd /home/ros2_ws/ \
&& . /opt/ros/foxy/setup.sh \
Expand Down
2 changes: 1 addition & 1 deletion ignition_ros2_control/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<depend>ament_index_cpp</depend>
<depend condition="$IGNITION_VERSION == cidadel">ignition-gazebo3</depend>
<depend condition="$IGNITION_VERSION == edifice">ignition-gazebo5</depend>
<depend>ignition-plugin1</depend>
<depend>ignition-plugin</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>yaml_cpp_vendor</depend>
Expand Down
52 changes: 22 additions & 30 deletions ignition_ros2_control/src/ignition_ros2_control_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
#include <ignition/gazebo/components/JointType.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/components/ParentEntity.hh>
#include <ignition/gazebo/components/Physics.hh>
#include <ignition/gazebo/components/World.hh>
#include <ignition/gazebo/Model.hh>

Expand Down Expand Up @@ -396,47 +395,40 @@ void IgnitionROS2ControlPlugin::Configure(
return;
}

auto worldEntity =
_ecm.EntityByComponents(ignition::gazebo::components::World());

auto physicsComp =
_ecm.Component<ignition::gazebo::components::Physics>(worldEntity);
const auto & physicsParams = physicsComp->Data();
const auto newStepSize =
std::chrono::duration<double>(physicsParams.MaxStepSize());

// Get the Gazebo simulation period
rclcpp::Duration gazebo_period(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(newStepSize)));

auto cm_update_rate = this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
this->dataPtr->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(cm_update_rate))));
// Check the period against the simulation period
if (this->dataPtr->control_period_ < gazebo_period) {
RCLCPP_ERROR_STREAM(
this->dataPtr->node->get_logger(),
"Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
" s) is faster than the gazebo simulation period (" <<
gazebo_period.seconds() << " s).");
} else if (this->dataPtr->control_period_ > gazebo_period) {
RCLCPP_WARN_STREAM(
this->dataPtr->node->get_logger(),
" Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
" s) is slower than the gazebo simulation period (" <<
gazebo_period.seconds() << " s).");
}

this->dataPtr->entity_ = _entity;
}

//////////////////////////////////////////////////
void IgnitionROS2ControlPlugin::PreUpdate(
const ignition::gazebo::UpdateInfo & /*_info*/,
const ignition::gazebo::UpdateInfo & _info,
ignition::gazebo::EntityComponentManager & /*_ecm*/)
{
static bool warned{false};
if (!warned) {
rclcpp::Duration gazebo_period(_info.dt);

// Check the period against the simulation period
if (this->dataPtr->control_period_ < _info.dt) {
RCLCPP_ERROR_STREAM(
this->dataPtr->node->get_logger(),
"Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
" s) is faster than the gazebo simulation period (" <<
gazebo_period.seconds() << " s).");
} else if (this->dataPtr->control_period_ > gazebo_period) {
RCLCPP_WARN_STREAM(
this->dataPtr->node->get_logger(),
" Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
" s) is slower than the gazebo simulation period (" <<
gazebo_period.seconds() << " s).");
}
warned = true;
}

// Always set commands on joints, otherwise at low control frequencies the joints tremble
// as they are updated at a fraction of gazebo sim time
this->dataPtr->controller_manager_->write();
Expand Down