From 3bab524526f67073390fb1e30a1bbfc886f63003 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20BUSSIERE?= <remi.bussiere@ecam.fr>
Date: Sun, 7 Jan 2024 23:24:33 +0100
Subject: [PATCH] src

---
 src/poppy_ros.cpp | 107 ++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 107 insertions(+)
 create mode 100644 src/poppy_ros.cpp

diff --git a/src/poppy_ros.cpp b/src/poppy_ros.cpp
new file mode 100644
index 0000000..1119ba3
--- /dev/null
+++ b/src/poppy_ros.cpp
@@ -0,0 +1,107 @@
+#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;
+
+void chatterCallback(const geometry_msgs::Twist::ConstPtr& msg)
+{
+   std::vector<uint16_t> position;
+   // Access linear and angular components from the message
+   position[0] = msg->linear.x;
+   position[1] = msg->linear.y;
+   position[2] = msg->linear.z;
+   
+   position[3] = msg->angular.x;
+   position[4] = msg->angular.y;
+   position[5] = msg->angular.z;
+   
+   _oDxlHandler.sendTargetJointPosition(position);
+}
+
+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);
+	ros::Subscriber sub = nh.subscribe("joint_cmd", 100, chatterCallback);
+	// 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;
+}