Skip to content

Changes to enable other controllers for Isaac Sim #37

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.

Sign up for GitHub

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

Open
karanchahal-nv wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Changes to enable other controllers for Isaac Sim #37

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
5 changes: 4 additions & 1 deletion include/topic_based_ros2_control/topic_based_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>

#include <std_msgs/msg/bool.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

namespace topic_based_ros2_control
Expand All @@ -68,8 +68,11 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
private:
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_states_subscriber_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr topic_based_joint_commands_publisher_;
// Add a new subscriber for a signal whether to stop publishing joint commands.
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stop_joint_commands_subscriber_;
rclcpp::Node::SharedPtr node_;
sensor_msgs::msg::JointState latest_joint_state_;
bool stop_joint_commands_{ false };
bool sum_wrapped_joint_states_{ false };

/// Use standard interfaces for joints because they are relevant for dynamic behavior
Expand Down
9 changes: 9 additions & 0 deletions src/topic_based_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,10 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
get_hardware_parameter("joint_states_topic", "/robot_joint_states"), rclcpp::SensorDataQoS(),
[this](const sensor_msgs::msg::JointState::SharedPtr joint_state) { latest_joint_state_ = *joint_state; });

stop_joint_commands_subscriber_ = node_->create_subscription<std_msgs::msg::Bool>(
get_hardware_parameter("stop_joint_commands_topic", "/stop_joint_commands"), rclcpp::QoS(1),
[this](const std_msgs::msg::Bool::SharedPtr stop_joint_commands) { stop_joint_commands_ = (*stop_joint_commands).data; });

// if the values on the `joint_states_topic` are wrapped between -2*pi and 2*pi (like they are in Isaac Sim)
// sum the total joint rotation returned on the `joint_states_` interface
if (get_hardware_parameter("sum_wrapped_joint_states", "false") == "true")
Expand Down Expand Up @@ -276,6 +280,11 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
return hardware_interface::return_type::OK;
}

if (stop_joint_commands_)
{
return hardware_interface::return_type::OK;
}

sensor_msgs::msg::JointState joint_state;
for (std::size_t i = 0; i < info_.joints.size(); ++i)
{
Expand Down