Save

poppy_ros.cpp

@@ -0,0 +1,88 @@
+#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;
+}