diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8c2b1b2..b60463c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,6 +32,8 @@ ament_target_dependencies(
rclcpp_lifecycle
)
+target_link_libraries(hexapod_robot PUBLIC i2c)
+
pluginlib_export_plugin_description_file(hardware_interface hexapod_robot.xml)
@@ -75,7 +77,9 @@ ament_export_dependencies(
hardware_interface
pluginlib
rclcpp
- rclcpp_lifecycle
+ rcl
+ cpp_lifecycle
+ i2c
)
diff --git a/bringup/config/controllers.yaml b/bringup/config/controllers.yaml
index f313b1f..2ec8177 100644
--- a/bringup/config/controllers.yaml
+++ b/bringup/config/controllers.yaml
@@ -1,32 +1,58 @@
controller_manager:
ros__parameters:
- update_rate: 10
+ update_rate: 50
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
-
+
+ joint_trajectory_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
forward_position_controller:
type: forward_command_controller/ForwardCommandController
+
+
forward_position_controller:
+ ros__parameters:
+ joints:
+ - t3_joint
+ interface_name: position
+
+joint_trajectory_controller:
ros__parameters:
joints:
- c1_joint
- - c2_joint
- - c3_joint
- - c4_joint
- - c5_joint
- - c6_joint
- f1_joint
- - f2_joint
- - f3_joint
- - f4_joint
- - f5_joint
- - f6_joint
- t1_joint
+
+ - c2_joint
+ - f2_joint
- t2_joint
+
+ - c3_joint
+ - f3_joint
- t3_joint
+
+ - c4_joint
+ - f4_joint
- t4_joint
+
+ - c5_joint
+ - f5_joint
- t5_joint
+
+ - c6_joint
+ - f6_joint
- t6_joint
- interface_name: position
+
+ command_interfaces:
+ - position
+
+ state_interfaces:
+ - position
+
+ action_monitor_rate: 20.0
+
+ allow_partial_joints_goal: false
+
diff --git a/bringup/launch/hexapod_robot.launch.py b/bringup/launch/hexapod_robot.launch.py
index 11c9a00..24dd10e 100644
--- a/bringup/launch/hexapod_robot.launch.py
+++ b/bringup/launch/hexapod_robot.launch.py
@@ -25,13 +25,6 @@ def generate_launch_description():
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
-
- control_node = Node(
- package = "controller_manager",
- executable = "ros2_control_node",
- parameters = [robot_description, robot_controllers],
- output="both"
- )
robot_state_publisher_node = Node(
package = "robot_state_publisher",
@@ -40,51 +33,69 @@ def generate_launch_description():
output = "both"
)
- joint_state_publisher_gui = Node(
- package="joint_state_publisher_gui",
- executable="joint_state_publisher_gui"
- )
-
- rviz_node = Node(
- package = "rviz2",
- executable = "rviz2",
- arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
- name = "rviz2",
- output = "screen",
+ control_node = Node(
+ package = "controller_manager",
+ executable = "ros2_control_node",
+ parameters = [robot_description, robot_controllers],
+ output="both"
)
+ # rviz_node = Node(
+ # package = "rviz2",
+ # executable = "rviz2",
+ # arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
+ # name = "rviz2",
+ # output = "screen",
+ # )
+
joint_state_broadcaster_spawner = Node (
package = "controller_manager",
executable = "spawner",
arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
)
- robot_controller_spawner = Node(
+ # forward_position_controller_spawner = Node(
+ # package = "controller_manager",
+ # executable = "spawner",
+ # arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
+ # )
+
+ joint_trajectory_controller_spawner = Node(
package = "controller_manager",
executable = "spawner",
- arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
- )
-
- delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
- event_handler = OnProcessExit(
- target_action = joint_state_broadcaster_spawner,
- on_exit = [rviz_node]
- )
+ arguments = ["joint_trajectory_controller", "--controller-manager", "/controller_manager"]
)
+
+
+ # delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+ # event_handler = OnProcessExit(
+ # target_action = joint_state_broadcaster_spawner,
+ # on_exit = [rviz_node]
+ # )
+ # )
- delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+ # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+ # event_handler = OnProcessExit(
+ # target_action = joint_state_broadcaster_spawner,
+ # on_exit = [forward_position_controller_spawner]
+ # )
+ # )
+
+ delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler = OnProcessExit(
target_action = joint_state_broadcaster_spawner,
- on_exit = [robot_controller_spawner]
+ on_exit = [joint_trajectory_controller_spawner]
)
)
-
+
+
nodes = [
control_node,
robot_state_publisher_node,
joint_state_broadcaster_spawner,
- delay_rviz_after_joint_state_broadcaster_spawner,
- delay_robot_controller_spawner_after_joint_state_broadcaster_spawner
+ #delay_rviz_after_joint_state_broadcaster_spawner,
+ # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner,
+ delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
]
return LaunchDescription(nodes)
diff --git a/description/urdf/hexapod_robot_macros.xacro b/description/urdf/hexapod_robot_macros.xacro
index 006dbd6..6b1991e 100644
--- a/description/urdf/hexapod_robot_macros.xacro
+++ b/description/urdf/hexapod_robot_macros.xacro
@@ -55,11 +55,12 @@
-
+
+
@@ -84,12 +85,13 @@
-->
-
+
+
@@ -141,12 +143,13 @@
-
+
+
diff --git a/description/urdf/hexapod_robot_ros2_control.xacro b/description/urdf/hexapod_robot_ros2_control.xacro
index 27b4606..bef9ca6 100644
--- a/description/urdf/hexapod_robot_ros2_control.xacro
+++ b/description/urdf/hexapod_robot_ros2_control.xacro
@@ -27,44 +27,49 @@
-
+
${i2c_address}
${channel}
+ ${counts_intersect}
+ ${counts_slope}
+
hexapod_robot/HexapodRobotHardwareInterface
/dev/i2c-1
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
+
+
diff --git a/hardware/hexapod_robot_hwi.cpp b/hardware/hexapod_robot_hwi.cpp
index 6bf3a47..b95d0e5 100644
--- a/hardware/hexapod_robot_hwi.cpp
+++ b/hardware/hexapod_robot_hwi.cpp
@@ -4,6 +4,18 @@
#include "rclcpp/rclcpp.hpp"
#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+extern "C" {
+ #include
+}
namespace hexapod_robot {
@@ -12,7 +24,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR;
}
-
+
RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
for (const hardware_interface::ComponentInfo &joint: info_.joints) {
@@ -21,30 +33,106 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
std::string i2c_address_parameter = joint.name + "_i2c_address";
std::string channel_parameter = joint.name + "_channel";
+ std::string counts_intersect_parameter = joint.name + "_counts_intersect";
+ std::string counts_slope_parameter = joint.name + "_counts_slope";
+
PWMOutput pwm_output;
- pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
- pwm_output.channel = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
+ pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
+ pwm_output.channel = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
+ pwm_output.counts_intersect = stod(info_.hardware_parameters[counts_intersect_parameter]);
+ pwm_output.counts_slope = stod(info_.hardware_parameters[counts_slope_parameter]);
- pca9685_controller_available_[pwm_output.i2c_address] = false;
+ pca_available_[pwm_output.i2c_address] = false;
joint_pwm_mapping_[joint.name] = pwm_output;
- RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel);
+ RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d. intersect: %.4f slope: %.4f", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel, pwm_output.counts_intersect, pwm_output.counts_slope);
}
- for (const auto& [i2c_address, available]: pca9685_controller_available_) {
- RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "require PCA9685 at i2c 0x%02x.", i2c_address);
- }
- i2c_available_ = false;
- i2c_fd_ = open(info_.hardware_parameters["i2c_device"].c_str(), O_RDWR);
+ i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]);
+ pca_initialize_();
+
+ return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+bool
+HexapodRobotHardwareInterface::i2c_open_(const std::string& device) {
+ i2c_fd_ = open(device.c_str(), O_RDWR);
if (i2c_fd_ < 0) {
RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());
- } else {
- i2c_available_ = true;
+ return false;
}
+ return true;
+}
- return hardware_interface::CallbackReturn::SUCCESS;
+void
+HexapodRobotHardwareInterface::pca_initialize_() {
+ for (const auto& [i2c_address, available]: pca_available_) {
+
+ ioctl(i2c_fd_, I2C_SLAVE, i2c_address);
+
+ if(i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, 0x00) < 0) {
+ RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Could not open i2c device %02x, marking as unavailable.", i2c_address);
+ pca_available_[i2c_address] = false;
+ } else {
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, pca_mode1_sleep_);
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_, pca_mode2_outdrv_);
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_prescale_, (uint8_t)pca_prescaler_value_);
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, pca_mode1_restart_);
+
+ RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "PCA9685 at i2c 0x%02x initialized.", i2c_address);
+ pca_available_[i2c_address] = true;
+ }
+ }
+}
+
+bool
+HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts) {
+ // if(i2c_address == 0x40 && channel_number == 0) {
+ // RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm(%02x, %02x, %04x)", i2c_address, channel_number, counts);
+ // }
+ if(channel_number>15) {
+ RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: channel %02x >15", channel_number);
+ return false;
+ }
+ if(counts > 0x1000) {
+ RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: counts %04x >15", counts);
+ return false;
+ }
+ if(!pca_available_[i2c_address]) {
+ // RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: i2c_address %02x unavailable.", i2c_address);
+ return false;
+ }
+
+ if(ioctl(i2c_fd_, I2C_SLAVE, i2c_address) < 0) {
+ RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: ioctl failed.");
+ return false;
+ }
+
+ // adding channel_number * 4 to the base address for LED0 output given by the constants
+ // yields the addresses of all PWM channels
+
+ if(counts == 0) {
+ // output always off
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00); // ON_L
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x00); // ON_H
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x10); // OFF_H == 0x10 => LED full OFF
+ } else if(counts == 0x1000) {
+ // output always on
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00); // ON_L
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x10); // ON_H == 0x10 => LED full ON
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x00); // OFF_H
+ } else {
+ // duty cycle from 0 to counts
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x00);
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00);
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, counts & 0xff); // lower byte
+ i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, counts >> 8); // shift upper byte into lower byte
+ }
+ return true;
}
hardware_interface::CallbackReturn
@@ -64,7 +152,7 @@ std::vector
HexapodRobotHardwareInterface::export_state_interfaces() {
std::vector state_interfaces;
for (const hardware_interface::ComponentInfo &joint: info_.joints) {
- state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_states_[joint.name]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
}
return state_interfaces;
}
@@ -81,14 +169,16 @@ HexapodRobotHardwareInterface::export_command_interfaces() {
hardware_interface::return_type
HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {
- for (const hardware_interface::ComponentInfo &joint: info_.joints) {
- joint_states_[joint.name]=joint_commands_[joint.name];
- }
return hardware_interface::return_type::OK;
}
hardware_interface::return_type
HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {
+ for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+ const PWMOutput &pwm_output = joint_pwm_mapping_[joint.name];
+ const double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope;
+ pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value);
+ }
return hardware_interface::return_type::OK;
}
diff --git a/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp b/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
index 092cd92..8435370 100644
--- a/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
+++ b/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
@@ -20,6 +20,8 @@ namespace hexapod_robot {
struct PWMOutput {
uint8_t i2c_address;
uint8_t channel;
+ double counts_intersect;
+ double counts_slope;
};
class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface {
@@ -39,15 +41,44 @@ public:
hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
private:
- int i2c_fd_;
+ std::map joint_commands_;
+
+ std::map joint_pwm_mapping_;
+
bool i2c_available_;
+ int i2c_fd_;
- std::map pca9685_controller_available_;
+ std::map pca_available_;
- std::map joint_states_;
- std::map joint_commands_;
+ bool i2c_open_(const std::string& device);
- std::map joint_pwm_mapping_;
+ const uint8_t pca_reg_mode1_ = 0x00;
+ const uint8_t pca_reg_mode2_ = 0x01;
+ const uint8_t pca_reg_prescale_ = 0xfe;
+
+ // base address for LED0 following,
+ // other channels can be calculated by adding
+ // channel_number * 4
+ // to each base address
+
+ const uint8_t pca_reg_led_on_l_ = 0x06;
+ const uint8_t pca_reg_led_on_h_ = 0x07;
+ const uint8_t pca_reg_led_off_l_ = 0x08;
+ const uint8_t pca_reg_led_off_h_ = 0x09;
+
+ const uint8_t pca_mode1_sleep_ = 0x10;
+ const uint8_t pca_mode1_restart_ = 0x80;
+ const uint8_t pca_mode2_outdrv_ = 0x04;
+
+
+ const double pca_osc_frequency_ = 25000000; // 25 Mhz
+ const double pca_pwm_frequency_ = 50; // 50 Hz
+ const double pca_prescaler_value_ = (pca_osc_frequency_ / (4096.0 * pca_pwm_frequency_) - 0.5); // according to data sheet
+ const double pca_pwm_cycle_usec_ = 1000000 / pca_pwm_frequency_; // microseconds per PWM counter cycle
+ const double pca_pwm_count_usec_ = pca_pwm_cycle_usec_ / 4096; // microseconds per PWM single count
+
+ void pca_initialize_();
+ bool pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts);
};