81 lines
2.8 KiB
C++
81 lines
2.8 KiB
C++
#include <ros/ros.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
#include <std_msgs/Float64MultiArray.h>
|
|
#include "DynamixelHandler.h"
|
|
#include <vector>
|
|
|
|
// Global variables for the Dynamixel handler and ROS publishers/subscribers
|
|
DynamixelHandler dxlHandler;
|
|
ros::Publisher jointPositionPublisher;
|
|
ros::Subscriber jointCommandSubscriber;
|
|
|
|
// Convert angles in degrees to Dynamixel motor commands
|
|
int convertAnglesToJointCmd(float fJointAngle) {
|
|
// Placeholder for conversion logic
|
|
// This should be replaced with the actual logic used in your system
|
|
// Example: Linear mapping from angle to motor command
|
|
const float minJointCmd = 0;
|
|
const float maxJointCmd = 1023;
|
|
const float minJointAngle = -180.0f;
|
|
const float maxJointAngle = 180.0f;
|
|
|
|
float a = (maxJointCmd - minJointCmd) / (maxJointAngle - minJointAngle);
|
|
float b = minJointCmd - a * minJointAngle;
|
|
return static_cast<int>(a * fJointAngle + b);
|
|
}
|
|
|
|
// Callback function for joint commands
|
|
void jointCMDCallback(const std_msgs::Float64MultiArray& joint_cmd) {
|
|
std::vector<uint16_t> target_positions;
|
|
for (float angle : joint_cmd.data) {
|
|
target_positions.push_back(static_cast<uint16_t>(convertAnglesToJointCmd(angle)));
|
|
}
|
|
|
|
// Send the target joint positions to the Dynamixel motors
|
|
dxlHandler.sendTargetJointPosition(target_positions);
|
|
}
|
|
|
|
int main(int argc, char** argv) {
|
|
ros::init(argc, argv, "poppy_ros");
|
|
ros::NodeHandle nh;
|
|
|
|
// Setup the DynamixelHandler
|
|
std::string poppyDxlPortName = "/dev/ttyUSB0";
|
|
float poppyDxlProtocol = 2.0;
|
|
int poppyDxlBaudRate = 1000000;
|
|
|
|
dxlHandler.setDeviceName(poppyDxlPortName);
|
|
dxlHandler.setProtocolVersion(poppyDxlProtocol);
|
|
dxlHandler.openPort();
|
|
dxlHandler.setBaudRate(poppyDxlBaudRate);
|
|
dxlHandler.enableTorque(true);
|
|
|
|
// Create the ROS publisher and subscriber
|
|
jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
|
|
jointCommandSubscriber = nh.subscribe("joint_cmd", 1, jointCMDCallback);
|
|
|
|
ros::Rate loop_rate(10); // Define a suitable rate
|
|
|
|
while (ros::ok()) {
|
|
// std::vector<uint16_t> current_positions = dxlHandler.readCurrentJointPosition();
|
|
std::vector<uint16_t> current_positions;
|
|
bool bIsReadSuccessfull = dxlHandler.readCurrentJointPosition(current_positions);
|
|
// Publish the current joint positions as Twist messages
|
|
geometry_msgs::Twist joint_positions_msg;
|
|
for (uint16_t position : current_positions) {
|
|
// Placeholder for converting Dynamixel units to degrees
|
|
// Add conversion logic here
|
|
}
|
|
|
|
jointPositionPublisher.publish(joint_positions_msg);
|
|
|
|
ros::spinOnce();
|
|
loop_rate.sleep();
|
|
}
|
|
|
|
dxlHandler.closePort();
|
|
|
|
return 0;
|
|
}
|
|
|