Skip to content

Commit

Permalink
ackermann almost done
Browse files Browse the repository at this point in the history
  • Loading branch information
@ARK3r
ARK3r committed Jun 29, 2023
1 parent d3c9831 commit 05d9eae
Show file tree
Hide file tree
Showing 4 changed files with 183 additions and 41 deletions.
6 changes: 3 additions & 3 deletions example_11/bringup/launch/carlikebot.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
"gui",
default_value="true",
description="Start RViz2 automatically with this launch file.",
)
Expand All @@ -41,7 +41,7 @@ def generate_launch_description():
)

# Initialize Arguments
start_rviz = LaunchConfiguration("start_rviz")
gui = LaunchConfiguration("gui")
sim = LaunchConfiguration("sim")

# Get URDF via xacro
Expand Down Expand Up @@ -101,7 +101,7 @@ def generate_launch_description():
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
condition=IfCondition(start_rviz),
condition=IfCondition(gui),
)

joint_state_broadcaster_spawner = Node(
Expand Down
2 changes: 1 addition & 1 deletion example_11/description/urdf/carlikebot.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<!-- 4 Wheel Robot with front steering and rear drive -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="carlikebot_robot">
<xacro:arg name="prefix" default="" />
<xacro:arg name="sim" default="false" />
<xacro:arg name="sim" default="true" />


<!-- Import Rviz colors -->
Expand Down
201 changes: 170 additions & 31 deletions example_11/hardware/carlikebot_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(

m_running_simulation = std::stod(info_.hardware_parameters["is_simulation"]);

// Check if the number of joints is correct based on the mode of operation
if (m_running_simulation && info_.joints.size() != 4)
{
RCLCPP_ERROR(
Expand All @@ -47,7 +48,6 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
info_.joints.size());
return hardware_interface::CallbackReturn::ERROR;
}

if (!m_running_simulation && info_.joints.size() != 2)
{
RCLCPP_ERROR(
Expand Down Expand Up @@ -82,27 +82,77 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
joint.state_interfaces.size());
return hardware_interface::CallbackReturn::ERROR;
}

// Check that the joints have the correct interfaces
// The steering joints have position interfaces and the drive joints velocity
if (joint.name.find("steering") != std::string::npos)
{
// Steering joints should have a single position command and state interface
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Joint '%s' has %s command interface. '%s' expected.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
}

if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Joint '%s' has %s state interface. '%s' expected.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION);
}
}
else
{
// Drive joints should have a single velocity command and state interface
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Joint '%s' has %s command interface. '%s' expected.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_VELOCITY);
}

if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Joint '%s' has %s state interface. '%s' expected.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_VELOCITY);
}
}


}

// The steering joints are controlled by position and the drive joints by velocity
// Running in simulation which means we have individual control of two front steering wheels and two rear drive wheels
// Running in simulation: we have individual control of two front steering wheels and two rear drive wheels
if (m_running_simulation)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
// END: This part here is for exemplary purposes - Please do not copy to your production code

// hw_positions_.resize(2, std::numeric_limits<double>::quiet_NaN());
// hw_velocities_.resize(2, std::numeric_limits<double>::quiet_NaN());
// hw_commands_.resize(4, std::numeric_limits<double>::quiet_NaN());
// Two steering joints and two drive joints
hw_positions_.resize(2, std::numeric_limits<double>::quiet_NaN());
hw_velocities_.resize(2, std::numeric_limits<double>::quiet_NaN());
// Which means 4 commands
hw_commands_.resize(4, std::numeric_limits<double>::quiet_NaN());
}
// Running on hardware which means we have a single front steering and a single rear drive motor
// Running on hardware: we have a single front steering and a single rear drive motor
else if (!m_running_simulation)
{
// hw_positions_.resize(1, std::numeric_limits<double>::quiet_NaN());
// hw_velocities_.resize(1, std::numeric_limits<double>::quiet_NaN());
// hw_commands_.resize(2, std::numeric_limits<double>::quiet_NaN());
// A single steering joint and a single drive joint
hw_positions_.resize(1, std::numeric_limits<double>::quiet_NaN());
hw_velocities_.resize(1, std::numeric_limits<double>::quiet_NaN());
// Which means 2 commands
hw_commands_.resize(2, std::numeric_limits<double>::quiet_NaN());
}

return hardware_interface::CallbackReturn::SUCCESS;
Expand All @@ -111,6 +161,9 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
std::vector<hardware_interface::StateInterface> CarlikeBotSystemHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;

// int commands_idx = 0;

for (auto i = 0u; i < info_.joints.size(); i++)
{
bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos;
Expand All @@ -124,36 +177,86 @@ std::vector<hardware_interface::StateInterface> CarlikeBotSystemHardware::export

if (is_steering_joint)
{
hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits<double>::quiet_NaN()));
// hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits<double>::quiet_NaN()));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_.at(info_.joints[i].name)));
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[0]));
// hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, commands_idx);
// commands_idx++;
}
else
{
hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits<double>::quiet_NaN()));
// hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits<double>::quiet_NaN()));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(info_.joints[i].name)));
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[0]));
// hw_names_.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, commands_idx);
// commands_idx++;
}
}

RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Exported %zu state interfaces.", state_interfaces.size()
);

for (auto s : state_interfaces)
{
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Exported state interface '%s'.", s.get_name().c_str());
}

return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
CarlikeBotSystemHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;

int position_commands_counter = 0;
int velocity_commands_counter = 0;
int commands_counter = 0;

for (auto i = 0u; i < info_.joints.size(); i++)
{
bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos;

auto hw_if = is_steering_joint ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY;
if (is_steering_joint)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_POSITION, position_commands_counter);
position_commands_counter++;
commands_counter++;
}
else
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
commands_counterpart_.emplace_back(commands_counter, hardware_interface::HW_IF_VELOCITY, velocity_commands_counter);
velocity_commands_counter++;
commands_counter++;
}
}

hw_commands_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits<double>::quiet_NaN()));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hw_if, &hw_commands_.at(info_.joints[i].name)));
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Exported %zu command interfaces.", command_interfaces.size()
);

for (int i = 0u; i < commands_positions_idx_.size(); i++)
{
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Exported position joint %d at command interface %d.", commands_positions_idx_[i].first, commands_positions_idx_[i].second);
}

for (int i = 0u; i < commands_velocities_idx_.size(); i++)
{
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"Exported velocity joint %d at command interface %d.", commands_velocities_idx_[i].first, commands_velocities_idx_[i].second);
}
return command_interfaces;
}

Expand All @@ -171,20 +274,19 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
}

// iterate through hw_positions_ map and set all second values to 0
for (auto it = hw_positions_.begin(); it != hw_positions_.end(); ++it)
for (auto i = 0u; i < hw_positions_.size(); i++)
{
it->second = 0;
hw_positions_[i] = 0.0;
}

for (auto it = hw_velocities_.begin(); it != hw_velocities_.end(); ++it)
for (auto i = 0u; i < hw_velocities_.size(); i++)
{
it->second = 0;
hw_velocities_[i] = 0.0;
}

for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it)
for (auto i = 0u; i < hw_commands_.size(); i++)
{
it->second = 0;
hw_commands_[i] = 0.0;
}
}

Expand Down Expand Up @@ -235,22 +337,59 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH

if (m_running_simulation)
{
for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it)
for (auto i = 0u; i < hw_commands_.size(); i++)
{
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it->second,
it->first.c_str());
rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i],
info_.joints[i].name.c_str());

}

for (auto i = 0u; i < hw_commands_.size(); i++)
{
// Find the tuple in commands_counterpart_ that has the first value of it equal to i
auto it = std::find_if(commands_counterpart_.begin(), commands_counterpart_.end(),
[i](const std::tuple<int, std::string, int> & element) { return std::get<0>(element) == i; });

// Get the interface_type from the second value of the tuple
std::string interface_type = std::get<1>(*it);

// Get the interface specific idx from the third value of the tuple
int interface_idx = std::get<2>(*it);

if (it->first.find("steering") != std::string::npos)
if (interface_type == "position")
{
hw_positions_.at(it->first) = it->second;
hw_positions_[interface_idx] = hw_commands_[i];
}
else
{
hw_velocities_.at(it->first) = it->second;
hw_velocities_[interface_idx] = hw_commands_[i];
}

RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully written to joint %d of type %s!", i, interface_type.c_str());

}



// for (auto it : hw_commands_)
// {
// RCLCPP_INFO(
// rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it.second,
// it.first.c_str());

// if

// if (it.first.find("steering") != std::string::npos)
// {
// hw_positions_.at(it.first) = it.second;
// }
// else
// {
// hw_velocities_.at(it.first) = it.second;
// }
// }
}

RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!");
Expand Down
15 changes: 9 additions & 6 deletions example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <string>
#include <vector>
#include <map>
#include <utility>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
Expand Down Expand Up @@ -76,13 +77,15 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface


// Store the command for the CarlikeBot robot
// std::vector<double> hw_commands_;
// std::vector<double> hw_positions_;
// std::vector<double> hw_velocities_;
std::vector<double> hw_commands_;
std::vector<double> hw_positions_;
std::vector<double> hw_velocities_;

std::map<std::string, double> hw_commands_;
std::map<std::string, double> hw_positions_;
std::map<std::string, double> hw_velocities_;
std::vector<std::tuple<int, std::string, int>> commands_counterpart_;

// std::map<std::string, double> hw_commands_;
// std::map<std::string, double> hw_positions_;
// std::map<std::string, double> hw_velocities_;
};

} // namespace ros2_control_demo_example_11
Expand Down

0 comments on commit 05d9eae

Please sign in to comment.