poppy_ros

poppy_ros/src/poppy_ros.cpp

@@ -0,0 +1,111 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "std_msgs/Float64MultiArray.msg"
+#include "DynamixelHandler.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;
+// create vector unit 16_t
+std::vector<uint16_t> jointVec;
+
+void jointCMDCallback(const geometry_msgs::Twist& joint_cmd)
+{
+	jointVec.clear();
+	//fill it w joint_cmd_values
+	jointVec.push_back(joint_cmd.linear.x);
+	jointVec.push_back(joint_cmd.linear.y);
+	jointVec.push_back(joint_cmd.linear.z);
+	jointVec.push_back(joint_cmd.angular.x);
+	jointVec.push_back(joint_cmd.angular.y);
+	jointVec.push_back(joint_cmd.angular.z);
+	//call sendTargetJOintPosition(vector) of DxlHandler 
+	_oDxlHandler.sendtargetJointPosition(jointVec);
+}
+
+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 subscriber
+	ros::Subscriber sub = nh.suscribe("joint_cmd",1, jointCMDCallback);
+	
+	// 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;
+}