Skip to content

Commit

Permalink
most recent
Browse files Browse the repository at this point in the history
  • Loading branch information
@AR2100
AR2100 committed Nov 12, 2024
1 parent 7616c8b commit 035ba1d
Show file tree
Hide file tree
Showing 4 changed files with 115 additions and 107 deletions.
1 change: 1 addition & 0 deletions external/libuvc-theta
Submodule libuvc-theta added at e4c978
1 change: 1 addition & 0 deletions external/ros2_thetav
Submodule ros2_thetav added at ad0207
126 changes: 66 additions & 60 deletions urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,52 +8,44 @@
#include <iterator>
#include <ostream>
#include <pb.h>
#include <pb_encode.h>
#include <pb_decode.h>
#include <pb_encode.h>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include <string>
#include <urc_nanopb/urc.pb.h>

PLUGINLIB_EXPORT_CLASS(
urc_hardware::hardware_interfaces::RoverDrivetrain,
hardware_interface::SystemInterface);
PLUGINLIB_EXPORT_CLASS(urc_hardware::hardware_interfaces::RoverDrivetrain,
hardware_interface::SystemInterface);

namespace urc_hardware::hardware_interfaces
{
namespace urc_hardware::hardware_interfaces {

RoverDrivetrain::RoverDrivetrain()
: hardware_interface_name("Rover Drivetrain")
, velocity_rps_commands(2, 0)
, velocity_rps_states(2, 0)
, velocity_rps_breakdown(6, 0)
{
}
: hardware_interface_name("Rover Drivetrain"), velocity_rps_commands(2, 0),
velocity_rps_states(2, 0), velocity_rps_breakdown(6, 0) {}
RoverDrivetrain::~RoverDrivetrain() = default;

hardware_interface::CallbackReturn RoverDrivetrain::on_init(
const hardware_interface::HardwareInfo & info)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_init(const hardware_interface::HardwareInfo &info) {
if (hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS)
{
hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR;
}

if (info_.hardware_parameters.find("udp_address") == info_.hardware_parameters.end()) {
RCLCPP_ERROR(
rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_address' configuration not "
"found. Expect to enter the udp server address.");
if (info_.hardware_parameters.find("udp_address") ==
info_.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_address' configuration not "
"found. Expect to enter the udp server address.");
return hardware_interface::CallbackReturn::ERROR;
}
if (info_.hardware_parameters.find("udp_port") == info_.hardware_parameters.end()) {
RCLCPP_ERROR(
rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_port' configuration not "
"found. Expect to enter the port number.");
if (info_.hardware_parameters.find("udp_port") ==
info_.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_port' configuration not "
"found. Expect to enter the port number.");
return hardware_interface::CallbackReturn::ERROR;
}

Expand All @@ -63,66 +55,75 @@ hardware_interface::CallbackReturn RoverDrivetrain::on_init(
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_configure(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_configure(const rclcpp_lifecycle::State &) {
std::fill(velocity_rps_commands.begin(), velocity_rps_commands.end(), 0.0);
std::fill(velocity_rps_states.begin(), velocity_rps_states.end(), 0.0);
std::fill(velocity_rps_breakdown.begin(), velocity_rps_breakdown.end(), 0.0);
return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::CommandInterface> RoverDrivetrain::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface>
RoverDrivetrain::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back("left_wheel", "velocity", &this->velocity_rps_commands[0]);
command_interfaces.emplace_back("right_wheel", "velocity", &this->velocity_rps_commands[1]);
command_interfaces.emplace_back("left_wheel", "velocity",
&this->velocity_rps_commands[0]);
command_interfaces.emplace_back("right_wheel", "velocity",
&this->velocity_rps_commands[1]);
return command_interfaces;
}

std::vector<hardware_interface::StateInterface> RoverDrivetrain::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface>
RoverDrivetrain::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back("left_wheel", "velocity", &this->velocity_rps_states[0]);
state_interfaces.emplace_back("right_wheel", "velocity", &this->velocity_rps_states[1]);
state_interfaces.emplace_back("left_wheel", "velocity.front", &this->velocity_rps_breakdown[0]);
state_interfaces.emplace_back("left_wheel", "velocity.mid", &this->velocity_rps_breakdown[1]);
state_interfaces.emplace_back("left_wheel", "velocity.back", &this->velocity_rps_breakdown[2]);
state_interfaces.emplace_back("right_wheel", "velocity.front", &this->velocity_rps_breakdown[3]);
state_interfaces.emplace_back("right_wheel", "velocity.mid", &this->velocity_rps_breakdown[4]);
state_interfaces.emplace_back("right_wheel", "velocity.back", &this->velocity_rps_breakdown[5]);
state_interfaces.emplace_back("left_wheel", "velocity",
&this->velocity_rps_states[0]);
state_interfaces.emplace_back("right_wheel", "velocity",
&this->velocity_rps_states[1]);
state_interfaces.emplace_back("left_wheel", "velocity.front",
&this->velocity_rps_breakdown[0]);
state_interfaces.emplace_back("left_wheel", "velocity.mid",
&this->velocity_rps_breakdown[1]);
state_interfaces.emplace_back("left_wheel", "velocity.back",
&this->velocity_rps_breakdown[2]);
state_interfaces.emplace_back("right_wheel", "velocity.front",
&this->velocity_rps_breakdown[3]);
state_interfaces.emplace_back("right_wheel", "velocity.mid",
&this->velocity_rps_breakdown[4]);
state_interfaces.emplace_back("right_wheel", "velocity.back",
&this->velocity_rps_breakdown[5]);
return state_interfaces;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_activate(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_activate(const rclcpp_lifecycle::State &) {
udp_ = std::make_shared<UDPSocket<128>>(true);
udp_->Connect(udp_address, std::stoi(udp_port));
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Rover Drivetrain activated!");
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name),
"Rover Drivetrain activated!");
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_deactivate(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_deactivate(const rclcpp_lifecycle::State &) {
udp_->Close();
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Rover Drivetrain deactivated!");
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name),
"Rover Drivetrain deactivated!");
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type RoverDrivetrain::read(
const rclcpp::Time &,
const rclcpp::Duration &)
{
hardware_interface::return_type
RoverDrivetrain::read(const rclcpp::Time &, const rclcpp::Duration &) {

// RCLCPP_INFO(rclcpp::get_logger("test"),
// "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);

return hardware_interface::return_type::OK;
}

hardware_interface::return_type RoverDrivetrain::write(
const rclcpp::Time & time,
const rclcpp::Duration & duration)
{
hardware_interface::return_type
RoverDrivetrain::write(const rclcpp::Time &time,
const rclcpp::Duration &duration) {
if (duration.seconds() < 0.001) {
return hardware_interface::return_type::OK;
}
Expand All @@ -132,10 +133,15 @@ hardware_interface::return_type RoverDrivetrain::write(
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));

SetpointMessage setpoint;
setpoint.leftSetpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
setpoint.rightSetpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
setpoint.leftSetpoint = 1 * ENCODER_CPR * -1;
setpoint.rightSetpoint = 1 * ENCODER_CPR * -1;
message.messageType.setpointMessage = setpoint;

RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "%d",
setpoint.leftSetpoint);
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "%d",
setpoint.rightSetpoint);
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "%d", ENCODER_CPR);

bool status = pb_encode(&stream, TeensyMessage_fields, &message);
message_length = stream.bytes_written;
Expand All @@ -148,4 +154,4 @@ hardware_interface::return_type RoverDrivetrain::write(
return hardware_interface::return_type::OK;
}

} // namespace urc_hardware::hardware_interfaces
} // namespace urc_hardware::hardware_interfaces
94 changes: 47 additions & 47 deletions urc_hw_description/urdf/ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@
<ros2_control name="rover_drivetrain" type="system">
<hardware>
<plugin>urc_hw/RoverDrivetrain</plugin>
<param name="udp_address">192.168.1.113</param>
<param name="udp_address">192.168.1.61</param>
<param name="udp_port">8443</param>
</hardware>
<joint name="left_wheel">
Expand All @@ -170,58 +170,58 @@
<ros2_control name="status_light" type="system">
<hardware>
<plugin>urc_hw/StatusLight</plugin>
<param name="udp_address">192.168.1.113</param>
<param name="udp_address">192.168.1.61</param>
<param name="udp_port">8443</param>
</hardware>
<gpio name="status_light">
<command_interface name="color" />
<command_interface name="state" />
</gpio>
</ros2_control>
<ros2_control name="battery_management" type="sensor">
<hardware>
<plugin>urc_hw/BatteryManagement</plugin>
<param name="udp_address">127.0.0.1</param>
<param name="udp_port">5000</param>
<param name="udp_self_address">127.0.0.1</param>
<param name="udp_self_port">7000</param>
</hardware>
<sensor name="battery_management">
<state_interface name="main_voltage" />
<state_interface name="charge_percentage" />
<state_interface name="discharge_current" />
<state_interface name="temperature" />
<state_interface name="cell1_voltage" />
<state_interface name="cell2_voltage" />
<state_interface name="cell3_voltage" />
<state_interface name="cell4_voltage" />
<state_interface name="cell5_voltage" />
<state_interface name="cell6_voltage" />
<state_interface name="cell7_voltage" />
<state_interface name="cell8_voltage" />
</sensor>
</ros2_control>
<ros2_control name="imu" type="sensor">
<hardware>
<plugin>urc_hw/IMU</plugin>
<param name="udp_address">127.0.0.1</param>
<param name="udp_port">5000</param>
<param name="udp_self_address">127.0.0.1</param>
<param name="udp_self_port">7000</param>
</hardware>
<sensor name="imu_sensor">
<state_interface name="orientation.x" />
<state_interface name="orientation.y" />
<state_interface name="orientation.z" />
<state_interface name="orientation.w" />
<state_interface name="linear_acceleration.x" />
<state_interface name="linear_acceleration.y" />
<state_interface name="linear_acceleration.z" />
<state_interface name="angular_velocity.x" />
<state_interface name="angular_velocity.y" />
<state_interface name="angular_velocity.z" />
</sensor>
</ros2_control>
<!-- <ros2_control name="battery_management" type="sensor"> -->
<!-- <hardware> -->
<!-- <plugin>urc_hw/BatteryManagement</plugin> -->
<!-- <param name="udp_address">127.0.0.1</param> -->
<!-- <param name="udp_port">5000</param> -->
<!-- <param name="udp_self_address">127.0.0.1</param> -->
<!-- <param name="udp_self_port">7000</param> -->
<!-- </hardware> -->
<!-- <sensor name="battery_management"> -->
<!-- <state_interface name="main_voltage" /> -->
<!-- <state_interface name="charge_percentage" /> -->
<!-- <state_interface name="discharge_current" /> -->
<!-- <state_interface name="temperature" /> -->
<!-- <state_interface name="cell1_voltage" /> -->
<!-- <state_interface name="cell2_voltage" /> -->
<!-- <state_interface name="cell3_voltage" /> -->
<!-- <state_interface name="cell4_voltage" /> -->
<!-- <state_interface name="cell5_voltage" /> -->
<!-- <state_interface name="cell6_voltage" /> -->
<!-- <state_interface name="cell7_voltage" /> -->
<!-- <state_interface name="cell8_voltage" /> -->
<!-- </sensor> -->
<!-- </ros2_control> -->
<!-- <ros2_control name="imu" type="sensor"> -->
<!-- <hardware> -->
<!-- <plugin>urc_hw/IMU</plugin> -->
<!-- <param name="udp_address">127.0.0.1</param> -->
<!-- <param name="udp_port">5000</param> -->
<!-- <param name="udp_self_address">127.0.0.1</param> -->
<!-- <param name="udp_self_port">7000</param> -->
<!-- </hardware> -->
<!-- <sensor name="imu_sensor"> -->
<!-- <state_interface name="orientation.x" /> -->
<!-- <state_interface name="orientation.y" /> -->
<!-- <state_interface name="orientation.z" /> -->
<!-- <state_interface name="orientation.w" /> -->
<!-- <state_interface name="linear_acceleration.x" /> -->
<!-- <state_interface name="linear_acceleration.y" /> -->
<!-- <state_interface name="linear_acceleration.z" /> -->
<!-- <state_interface name="angular_velocity.x" /> -->
<!-- <state_interface name="angular_velocity.y" /> -->
<!-- <state_interface name="angular_velocity.z" /> -->
<!-- </sensor> -->
<!-- </ros2_control> -->
</xacro:if>
</xacro:macro>
</robot>
</robot>

0 comments on commit 035ba1d

Please sign in to comment.