Skip to content
Snippets Groups Projects
Commit d97e2c5d authored by Galambos Mihály's avatar Galambos Mihály
Browse files

Updated driver

parent 9feb718c
No related branches found
No related tags found
No related merge requests found
...@@ -5,3 +5,30 @@ serial_bridge: ...@@ -5,3 +5,30 @@ serial_bridge:
flow_control: none flow_control: none
parity: none parity: none
stop_bits: "1" stop_bits: "1"
log_level: info
odometry_publisher:
ros__parameters:
publish_frequency: 100.0
update_frequency: 50.0
ticks_meter_left: 3291
ticks_meter_right: 3319
encoder_ticks_min: -2147483648
encoder_ticks_max: 2147483648
encoder_low_wrap_threshold: 0.3
encoder_high_wrap_threshold: 0.7
base_width: 0.552
base_frame_id: base_link
odom_frame_id: odom
log_level: info
command_code_generator:
ros__parameters:
publish_frequency: 100.0
twist_topic_name: /hollander/cmd_vel
serial_out_topic_name: /hollander/serial/write
button_topic_name: /hollander/buttons
button_push_service_name: /hollander/push_button
x_vel_scale: 100.0
y_vel_scale: 100.0
log_level: info
serial_bridge:
ros__parameters:
device_name: /dev/black_pill
baud_rate: 9600
flow_control: none
parity: none
stop_bits: "1"
...@@ -19,7 +19,7 @@ def generate_launch_description(): ...@@ -19,7 +19,7 @@ def generate_launch_description():
# Get launch configuration # Get launch configuration
launch_dir = get_package_share_directory('hollander_bringup') launch_dir = get_package_share_directory('hollander_bringup')
config = os.path.join(launch_dir, 'config', 'driver.params.yaml') default_config_file = os.path.join(launch_dir, 'config', 'driver.config.yaml')
# Local variables # Local variables
bridge_node_name = 'serial_bridge' bridge_node_name = 'serial_bridge'
...@@ -27,11 +27,11 @@ def generate_launch_description(): ...@@ -27,11 +27,11 @@ def generate_launch_description():
code_node_name = 'command_code_generator' code_node_name = 'command_code_generator'
# Parameters # Parameters
params_declare = DeclareLaunchArgument( configurable_parameters = [{'name': 'config', 'default': default_config_file, 'description': 'File path to the ROS2 parameters file to use'},
'params_file', {'name': 'log_level', 'default': "info", 'description': '[0-None, 1-copy, 2-linear_interpolation]'}]
default_value=config,
description='File path to the ROS2 parameters file to use' def declare_configurable_parameters(parameters):
) return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
""" NODES """ """ NODES """
# Serial bridge # Serial bridge
...@@ -40,7 +40,7 @@ def generate_launch_description(): ...@@ -40,7 +40,7 @@ def generate_launch_description():
executable='serial_bridge', executable='serial_bridge',
name=bridge_node_name, name=bridge_node_name,
namespace=TextSubstitution(text=''), namespace=TextSubstitution(text=''),
parameters=[LaunchConfiguration('params_file')], parameters=[LaunchConfiguration('config')],
remappings=[ remappings=[
('serial_read', 'hollander/serial/read'), ('serial_read', 'hollander/serial/read'),
('serial_write', 'hollander/serial/write'), ('serial_write', 'hollander/serial/write'),
...@@ -54,7 +54,8 @@ def generate_launch_description(): ...@@ -54,7 +54,8 @@ def generate_launch_description():
executable='odometry_publisher_node', executable='odometry_publisher_node',
name=odom_node_name, name=odom_node_name,
namespace=TextSubstitution(text=''), namespace=TextSubstitution(text=''),
# arguments=['--ros-args', '--log-level', "debug"], parameters=[LaunchConfiguration('config')],
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
output='screen', output='screen',
) )
...@@ -64,7 +65,8 @@ def generate_launch_description(): ...@@ -64,7 +65,8 @@ def generate_launch_description():
executable='command_code_generator_node', executable='command_code_generator_node',
name=code_node_name, name=code_node_name,
namespace=TextSubstitution(text=''), namespace=TextSubstitution(text=''),
# arguments=['--ros-args', '--log-level', "debug"], parameters=[LaunchConfiguration('config')],
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
output='screen', output='screen',
) )
...@@ -198,10 +200,7 @@ def generate_launch_description(): ...@@ -198,10 +200,7 @@ def generate_launch_description():
) )
) )
return LaunchDescription([ return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
# Declare all parameters
params_declare,
# Start the nodes # Start the nodes
command_code_generator_node, command_code_generator_node,
odometry_publisher_node, odometry_publisher_node,
......
...@@ -106,6 +106,9 @@ class CommandCodeGenerator : public lc::LifecycleNode { ...@@ -106,6 +106,9 @@ class CommandCodeGenerator : public lc::LifecycleNode {
// --- PARAMETERS --- // --- PARAMETERS ---
double publish_frequency; // The rate at which to update the odometry [Hz] double publish_frequency; // The rate at which to update the odometry [Hz]
double x_vel_scale;
double y_vel_scale;
std::string twist_topic_name; // The name of the twist message topic from which the node will update std::string twist_topic_name; // The name of the twist message topic from which the node will update
std::string serial_out_topic_name; // The name of the serial message topic where the node will publish std::string serial_out_topic_name; // The name of the serial message topic where the node will publish
std::string button_topic_name; // The name of the button topic where listen std::string button_topic_name; // The name of the button topic where listen
......
...@@ -59,8 +59,8 @@ class OdometryPublisher : public lc::LifecycleNode { ...@@ -59,8 +59,8 @@ class OdometryPublisher : public lc::LifecycleNode {
int encoder_ticks_min; // The smallest possible number for encoder ticks int encoder_ticks_min; // The smallest possible number for encoder ticks
int encoder_ticks_max; // The largest possible number for encoder ticks int encoder_ticks_max; // The largest possible number for encoder ticks
double encoder_low_wrap; // The threshold for overflow detection double encoder_low_wrap_threshold; // The threshold for overflow detection
double encoder_high_wrap; // The threshold for underflow detection double encoder_high_wrap_threshold; // The threshold for underflow detection
double base_width; // The width of the wheel base [m] double base_width; // The width of the wheel base [m]
...@@ -79,9 +79,13 @@ class OdometryPublisher : public lc::LifecycleNode { ...@@ -79,9 +79,13 @@ class OdometryPublisher : public lc::LifecycleNode {
double prev_ticks_left = 0.0; // The number of the left ticks in the previous iteration double prev_ticks_left = 0.0; // The number of the left ticks in the previous iteration
double prev_ticks_right = 0.0; // The number of the tight ticks in the previous iteration double prev_ticks_right = 0.0; // The number of the tight ticks in the previous iteration
double encoder_low_wrap; // The threshold for overflow detection
double encoder_high_wrap; // The threshold for underflow detection
double left_mult = 0.0; // Overflow multiplier for the left encoder double left_mult = 0.0; // Overflow multiplier for the left encoder
double right_mult = 0.0; // Overflow multiplier for the right encoder double right_mult = 0.0; // Overflow multiplier for the right encoder
// Twist // Twist
double x = 0.0; // x position in the xy plane [m] double x = 0.0; // x position in the xy plane [m]
double y = 0.0; // y position in the xy plane [m] double y = 0.0; // y position in the xy plane [m]
......
...@@ -80,6 +80,12 @@ void CommandCodeGenerator::declare_all_parameters() { ...@@ -80,6 +80,12 @@ void CommandCodeGenerator::declare_all_parameters() {
this->declare_parameter("button_push_service_name", "/hollander/push_button"); this->declare_parameter("button_push_service_name", "/hollander/push_button");
this->get_parameter("button_push_service_name", button_push_service_name); this->get_parameter("button_push_service_name", button_push_service_name);
this->declare_parameter("x_vel_scale", 100.0);
this->get_parameter("x_vel_scale", x_vel_scale);
this->declare_parameter("y_vel_scale", 100.0);
this->get_parameter("y_vel_scale", y_vel_scale);
} }
unsigned int CommandCodeGenerator::CommandCode::encode() { unsigned int CommandCodeGenerator::CommandCode::encode() {
...@@ -105,17 +111,24 @@ unsigned int CommandCodeGenerator::CommandCode::encode() { ...@@ -105,17 +111,24 @@ unsigned int CommandCodeGenerator::CommandCode::encode() {
void CommandCodeGenerator::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) { void CommandCodeGenerator::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
// Get the velocity values from the message // Get the velocity values from the message
auto twist = *msg; auto twist = *msg;
double vel_x = twist.linear.x; // [m/s] double r = twist.linear.x; // [m/s]
double vel_th = twist.angular.z; // [rad/s] double phi = twist.angular.z; // [rad/s]
// COMMAND CODE LOCK // COMMAND CODE LOCK
command_code_mutex.lock(); command_code_mutex.lock();
{ {
// TODO: Refactor the conversion RCLCPP_INFO(this->get_logger(), "r: %f | phi: %f", r, phi);
code.setStraightDir((vel_x >= 0) ? FORWARD : BACKWARD);
code.setStraightSpeed(abs(vel_x) * 100); // Convert polar to cartesian
code.setSideDir((vel_th > 0) ? LEFT : RIGHT); // double x = r * cos(phi);
code.setSideSpeed(abs(vel_th) * 100); // double y = r * sin(phi);
// RCLCPP_INFO(this->get_logger(), "x: %f | y: %f", x, y);
code.setStraightDir((r >= 0) ? FORWARD : BACKWARD);
code.setStraightSpeed(abs(r) * x_vel_scale);
code.setSideDir((phi > 0) ? LEFT : RIGHT);
code.setSideSpeed(abs(phi) * y_vel_scale);
} }
command_code_mutex.unlock(); command_code_mutex.unlock();
// COMMAND CODE UNLOCK // COMMAND CODE UNLOCK
......
...@@ -78,11 +78,13 @@ void OdometryPublisher::declare_all_parameters() { ...@@ -78,11 +78,13 @@ void OdometryPublisher::declare_all_parameters() {
this->declare_parameter("encoder_ticks_max", 2147483648); this->declare_parameter("encoder_ticks_max", 2147483648);
this->get_parameter("encoder_ticks_max", encoder_ticks_max); this->get_parameter("encoder_ticks_max", encoder_ticks_max);
this->declare_parameter("encoder_low_wrap", ((encoder_ticks_max - encoder_ticks_min) * 0.3) + encoder_ticks_min); this->declare_parameter("encoder_low_wrap_threshold", 0.3);
this->get_parameter("encoder_low_wrap", encoder_low_wrap); this->get_parameter("encoder_low_wrap_threshold", encoder_low_wrap_threshold);
this->encoder_low_wrap = ((encoder_ticks_max - encoder_ticks_min) * encoder_low_wrap_threshold) + encoder_ticks_min;
this->declare_parameter("encoder_high_wrap", ((encoder_ticks_max - encoder_ticks_min) * 0.7) + encoder_ticks_min); this->declare_parameter("encoder_high_wrap_threshold", 0.7);
this->get_parameter("encoder_high_wrap", encoder_high_wrap); this->get_parameter("encoder_high_wrap_threshold", encoder_high_wrap_threshold);
this->encoder_high_wrap = ((encoder_ticks_max - encoder_ticks_min) * encoder_high_wrap_threshold) + encoder_ticks_min;
this->declare_parameter("base_width", 0.552); this->declare_parameter("base_width", 0.552);
this->get_parameter("base_width", base_width); this->get_parameter("base_width", base_width);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment