89 lines
2.5 KiB
C++
89 lines
2.5 KiB
C++
#include <ros/ros.h>
|
|
#include <std_msgs/String.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
#include <vector>
|
|
#include <cstdint> // For uint16_t
|
|
|
|
#include "DynamixelHandler.h"
|
|
|
|
std::string _poppyDxlPortName = "/dev/ttyUSB0";
|
|
float _poppyDxlProtocol = 2.0;
|
|
int _poppyDxlBaudRate = 1000000;
|
|
|
|
DynamixelHandler Dxl;
|
|
|
|
void jointCommandCallback(const geometry_msgs::Twist& joint_cmd)
|
|
{
|
|
// Create a vector of uint16_t
|
|
std::vector<uint16_t> joint_values;
|
|
|
|
joint_values.push_back(joint_cmd.linear.x);
|
|
joint_values.push_back(joint_cmd.linear.y);
|
|
joint_values.push_back(joint_cmd.linear.z);
|
|
|
|
joint_values.push_back(joint_cmd.angular.x);
|
|
joint_values.push_back(joint_cmd.angular.y);
|
|
joint_values.push_back(joint_cmd.angular.z);
|
|
|
|
Dxl.sendTargetJointPosition(joint_values);
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "poppy_ros");
|
|
ros::NodeHandle nh;
|
|
|
|
ros::Subscriber sub = nh.subscribe("joint_cmd", 1, jointCommandCallback);
|
|
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
|
|
|
|
ros::Rate loopRate(10);
|
|
|
|
// create a Twist message
|
|
geometry_msgs::Twist jointPositionMsg;
|
|
|
|
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
|
Dxl.setDeviceName(_poppyDxlPortName);
|
|
Dxl.setProtocolVersion(_poppyDxlProtocol);
|
|
Dxl.openPort();
|
|
Dxl.setBaudRate(_poppyDxlBaudRate);
|
|
Dxl.enableTorque(false);
|
|
std::cout << std::endl;
|
|
|
|
|
|
|
|
ROS_INFO("===Launching Poppy node===");
|
|
|
|
// loop until Ctrl+C is pressed or ROS connectivity issues
|
|
while(ros::ok())
|
|
{
|
|
//===RETRIEVE Dynamixel Motor positions====
|
|
std::vector<uint16_t> l_vCurrentJointPosition;
|
|
bool bIsReadSuccessfull = Dxl.readCurrentJointPosition(l_vCurrentJointPosition);
|
|
|
|
// stores them into a msg
|
|
if (bIsReadSuccessfull)
|
|
{
|
|
jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
|
|
jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
|
|
jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
|
|
jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
|
|
jointPositionMsg.angular.y = l_vCurrentJointPosition[4];
|
|
jointPositionMsg.angular.z = l_vCurrentJointPosition[5];
|
|
}
|
|
|
|
// publish the Twist message to the joint_position topic
|
|
pub.publish(jointPositionMsg);
|
|
|
|
// spin once to let the process handle callback ad key stroke
|
|
ros::spinOnce();
|
|
|
|
// sleep the right amout of time to comply with _fps
|
|
loopRate.sleep();
|
|
}
|
|
|
|
Dxl.enableTorque(false);
|
|
Dxl.closePort();
|
|
|
|
return 0;
|
|
}
|