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.

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

Jump to bottom

Add ROS 2 jazzy #20

Open
wants to merge 3 commits into
base: ros2
Choose a base branch
from
Open

Add ROS 2 jazzy #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fc6aff3
Update ros-docker-image.yaml
delihus Jul 23, 2024
adf863b
Added stamped param
delihus Jul 23, 2024
120b714
add config
delihus Jul 23, 2024
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/ros-docker-image.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ jobs:
strategy:
fail-fast: false
matrix:
ros_distro: ["humble", "iron"]
ros_distro: ["humble", "iron", "jazzy"]

steps:

Expand Down
2 changes: 2 additions & 0 deletions joy2twist/config/joy2twist.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
/**:
ros__parameters:
cmd_vel.use_stamped: false

linear_velocity_factor:
fast: 1.0
regular: 0.5
Expand Down
2 changes: 2 additions & 0 deletions joy2twist/config/joy2twist_panther.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
/**:
ros__parameters:
cmd_vel.use_stamped: false

linear_velocity_factor:
fast: 2.0
regular: 1.2
Expand Down
24 changes: 24 additions & 0 deletions joy2twist/config/joy2twist_stamped.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
/**:
ros__parameters:
cmd_vel.use_stamped: true

linear_velocity_factor:
fast: 1.0
regular: 0.5
slow: 0.2

angular_velocity_factor:
fast: 1.0
regular: 0.5
slow: 0.2

# This button mapping should be adjusted to the specific controller
# The following map is suited for Logitech F710
button_index_map:
axis:
angular_z: 2 # Right joystick
linear_x: 1 # Left joystick
linear_y: 0 # Left joystick
dead_man_switch: 4 # LB
fast_mode: 7 # RT
slow_mode: 5 # RB
5 changes: 5 additions & 0 deletions joy2twist/include/joy2twist/joy2twist_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_srvs/srv/trigger.hpp>
Expand All @@ -16,6 +17,7 @@ namespace joy2twist
{
using MsgJoy = sensor_msgs::msg::Joy;
using MsgTwist = geometry_msgs::msg::Twist;
using MsgTwistStamped = geometry_msgs::msg::TwistStamped;
using MsgBool = std_msgs::msg::Bool;
using SrvTrigger = std_srvs::srv::Trigger;

Expand Down Expand Up @@ -51,11 +53,13 @@ class Joy2TwistNode : public rclcpp::Node
void trigger_service_cb(
const rclcpp::Client<SrvTrigger>::SharedFuture & future,
const std::string & service_name) const;
void publish_twist(const MsgTwist & twist_msg);

std::map<std::string, float> linear_velocity_factors_;
std::map<std::string, float> angular_velocity_factors_;

ButtonIndex button_index_;
bool use_stamped_cmd_vel_;
bool driving_mode_;
bool e_stop_present_;
bool e_stop_state_;
Expand All @@ -66,6 +70,7 @@ class Joy2TwistNode : public rclcpp::Node
rclcpp::Subscription<MsgBool>::SharedPtr e_stop_sub_;
rclcpp::Subscription<MsgJoy>::SharedPtr joy_sub_;
rclcpp::Publisher<MsgTwist>::SharedPtr twist_pub_;
rclcpp::Publisher<MsgTwistStamped>::SharedPtr twist_stamped_pub_;
rclcpp::Client<SrvTrigger>::SharedPtr e_stop_reset_client_;
rclcpp::Client<SrvTrigger>::SharedPtr e_stop_trigger_client_;
};
Expand Down
33 changes: 27 additions & 6 deletions joy2twist/src/joy2twist_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,14 @@ Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node")

joy_sub_ = create_subscription<MsgJoy>(
"joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1));
twist_pub_ = create_publisher<MsgTwist>(
"cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());

if(use_stamped_cmd_vel_){
twist_stamped_pub_ = create_publisher<MsgTwistStamped>(
"cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
} else {
twist_pub_ = create_publisher<MsgTwist>(
"cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
}

if (e_stop_present_) {
e_stop_sub_ = this->create_subscription<MsgBool>(
Expand All @@ -29,6 +35,8 @@ Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node")

void Joy2TwistNode::declare_parameters()
{
this->declare_parameter<bool>("cmd_vel.use_stamped", false);

this->declare_parameter<float>("linear_velocity_factor.fast", 1.0);
this->declare_parameter<float>("linear_velocity_factor.regular", 0.5);
this->declare_parameter<float>("linear_velocity_factor.slow", 0.2);
Expand All @@ -54,6 +62,8 @@ void Joy2TwistNode::declare_parameters()

void Joy2TwistNode::load_parameters()
{
this->get_parameter<bool>("cmd_vel.use_stamped", use_stamped_cmd_vel_);

this->get_parameter<float>("linear_velocity_factor.fast", linear_velocity_factors_[FAST]);
this->get_parameter<float>("linear_velocity_factor.regular", linear_velocity_factors_[REGULAR]);
this->get_parameter<float>("linear_velocity_factor.slow", linear_velocity_factors_[SLOW]);
Expand All @@ -73,7 +83,7 @@ void Joy2TwistNode::load_parameters()
this->get_parameter<int>("button_index_map.fast_mode", button_index_.fast_mode);
this->get_parameter<int>("button_index_map.slow_mode", button_index_.slow_mode);
this->get_parameter<int>("button_index_map.e_stop_reset", button_index_.e_stop_reset);
this->get_parameter<int>("button_index_map.e_stop_trigger", button_index_.e_stop_trigger);
this->get_parameter<int>("button_index_map.e_suse_stamped_cmd_vel_top_trigger", button_index_.e_stop_trigger);
this->get_parameter<int>(
"button_index_map.enable_e_stop_reset", button_index_.enable_e_stop_reset);
}
Expand All @@ -87,7 +97,7 @@ void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
if (e_stop_present_) {
if (joy_msg->buttons.at(button_index_.e_stop_trigger) && !e_stop_state_) {
// Stop the robot before trying to call the e-stop trigger service
twist_pub_->publish(twist_msg);
publish_twist(twist_msg);
call_trigger_service(e_stop_trigger_client_);
} else if (
joy_msg->buttons.at(button_index_.enable_e_stop_reset) &&
Expand All @@ -99,10 +109,10 @@ void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
if (joy_msg->buttons.at(button_index_.dead_man_switch)) {
driving_mode_ = true;
convert_joy_to_twist(joy_msg, twist_msg);
twist_pub_->publish(twist_msg);
publish_twist(twist_msg);
} else if (driving_mode_) {
driving_mode_ = false;
twist_pub_->publish(twist_msg);
publish_twist(twist_msg);
}
}

Expand Down Expand Up @@ -159,4 +169,15 @@ void Joy2TwistNode::trigger_service_cb(
RCLCPP_INFO(this->get_logger(), "Successfully called %s service", service_name.c_str());
}

void Joy2TwistNode::publish_twist(const MsgTwist & twist_msg){
if(use_stamped_cmd_vel_){
MsgTwistStamped twist_stamped_msg;
twist_stamped_msg.header.stamp = this->get_clock()->now();
twist_stamped_msg.twist = twist_msg;
twist_stamped_pub_->publish(twist_stamped_msg);
} else {
twist_pub_->publish(twist_msg);
}
}

} // namespace joy2twist
Loading