110 lines
3.2 KiB
C++
110 lines
3.2 KiB
C++
#include "DynamixelHandler.h"
|
|
#include <ros/ros.h>
|
|
#include <geometry_msgs/Twist.h>
|
|
|
|
|
|
// Global variables
|
|
float _fps = 10.0f; // Hz
|
|
|
|
DynamixelHandler dxHandler;
|
|
ros::Publisher _jointPositionPublisher;
|
|
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;
|
|
}
|
|
|
|
void jointCallback(const geometry_msgs::Twist & joint_cmd){
|
|
|
|
int joint1 = convertAnglesToJointCmd(joint_cmd.linear.x);
|
|
int joint2 = convertAnglesToJointCmd(joint_cmd.linear.y);
|
|
int joint3 = convertAnglesToJointCmd(joint_cmd.linear.z);
|
|
int joint4 = convertAnglesToJointCmd(joint_cmd.angular.x);
|
|
int joint5 = convertAnglesToJointCmd(joint_cmd.angular.y);
|
|
int joint6 = convertAnglesToJointCmd(joint_cmd.angular.z);
|
|
|
|
std::vector <uint16_t> jointvec;
|
|
|
|
jointvec.push_back(joint1);
|
|
jointvec.push_back(joint2);
|
|
jointvec.push_back(joint3);
|
|
jointvec.push_back(joint4);
|
|
jointvec.push_back(joint5);
|
|
jointvec.push_back(joint6);
|
|
|
|
dxHandler.sendTargetJointPosition(jointvec);
|
|
|
|
}
|
|
|
|
|
|
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;
|
|
dxHandler.setDeviceName("/dev/ttyUSB0");
|
|
dxHandler.setProtocolVersion(2.0);
|
|
dxHandler.openPort();
|
|
dxHandler.setBaudRate(1000000);
|
|
dxHandler.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 = dxHandler.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();
|
|
}
|
|
|
|
dxHandler.enableTorque(false);
|
|
dxHandler.closePort();
|
|
|
|
return 0;
|
|
}
|