96 lines
2.6 KiB
C++
96 lines
2.6 KiB
C++
#include "DynamixelHandler.h"
|
|
|
|
#include <ros/ros.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
|
|
|
|
// Global variables
|
|
float _fps = 10.0f; // Hz
|
|
|
|
DynamixelHandler _oDxlHandler;
|
|
ros::Publisher _jointPositionPublisher;
|
|
std::string _poppyDxlPortName = "/dev/ttyUSB0";
|
|
float _poppyDxlProtocol = 2.0;
|
|
int _poppyDxlBaudRate = 1000000;
|
|
int _nbJoints = 6;
|
|
float _minJointCmd = 0;
|
|
float _maxJointCmd = 1023;
|
|
float _minJointAngle = -180.0f;
|
|
float _maxJointAngle = 180.0f;
|
|
|
|
|
|
|
|
int convertAnglesToJointCmd(float fJointAngle)
|
|
{
|
|
// y = ax + b
|
|
float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
|
|
float b = _minJointCmd - a * _minJointAngle;
|
|
float jointCmd = a * fJointAngle + b;
|
|
return (int)jointCmd;
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
// create a node called poppy_ros
|
|
ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
|
|
//ros::init(argc, argv, "autopilot");
|
|
|
|
// create a node handle
|
|
ros::NodeHandle nh;
|
|
|
|
// create a publisher to joint_position topic
|
|
_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
|
|
|
|
// create a loop rate
|
|
ros::Rate loopRate(_fps);
|
|
|
|
// create a Twist message
|
|
geometry_msgs::Twist jointPositionMsg;
|
|
|
|
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
|
_oDxlHandler.setDeviceName(_poppyDxlPortName);
|
|
_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
|
|
_oDxlHandler.openPort();
|
|
_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
|
|
_oDxlHandler.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 = _oDxlHandler.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
|
|
_jointPositionPublisher.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();
|
|
}
|
|
|
|
_oDxlHandler.enableTorque(false);
|
|
_oDxlHandler.closePort();
|
|
|
|
return 0;
|
|
}
|