Add support for velocity/effort mode via ROS bridge

examples/ros/motor_and_io_bridge.cpp

@@ -35,7 +35,7 @@
 #include "joint_state_subscriber.h"
 #include "entry_publisher.h"
 #include "entry_subscriber.h"
- 
+
 #include <thread>
 #include <chrono>
 #include <memory>
@@ -117,6 +117,7 @@ int main(int argc, char* argv[]) {
 
 			found = true;
 
+			// Choose between "profile_position_mode", "profile_velocity_mode", or "torque_mode"
 			PRINT("Set position mode");
 			device.set_entry("modes_of_operation", device.get_constant("profile_position_mode"));
 
@@ -125,10 +126,10 @@ int main(int argc, char* argv[]) {
 
 			// TODO: target_position should be mapped to a PDO
 
-			auto jspub = std::make_shared<kaco::JointStatePublisher>(device, 0, 350000);
+			auto jspub = std::make_shared<kaco::JointStatePublisher>(device, 0, 350000, 1, 60);
 			bridge.add_publisher(jspub, loop_rate);
-			
-			auto jssub = std::make_shared<kaco::JointStateSubscriber>(device, 0, 350000);
+
+			auto jssub = std::make_shared<kaco::JointStateSubscriber>(device, 0, 350000, 1, 60);
 			bridge.add_subscriber(jssub);
 
 		}

master/src/master.cpp

@@ -28,7 +28,7 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
- 
+
 #include "master.h"
 #include "core.h"
 #include "logger.h"
@@ -93,7 +93,7 @@ void Master::device_alive_callback(const uint8_t node_id) {
 	} else {
 		WARN("Device with node ID "<<node_id<<" already exists. Ignoring...");
 	}
-	
+
 }
 
 

master/src/profiles.cpp

@@ -76,6 +76,22 @@ namespace kaco {
 						device.execute("unset_controlword_flag","controlword_pp_new_set_point");
 						return Value();
 					}
+				},
+				{
+					"set_target_velocity",
+					[](Device& device,const Value& target_velocity) -> Value {
+						DEBUG_LOG("Set target vel to "<<target_velocity);
+						device.set_entry("Target velocity", target_velocity);
+						return Value();
+					}
+				},
+				{
+					"set_target_torque",
+					[](Device& device,const Value& target_torque) -> Value {
+						DEBUG_LOG("Set target tor to "<<target_torque);
+						device.set_entry("Target torque", target_torque);
+						return Value();
+					}
 				}
 			}
 		}
@@ -112,15 +128,15 @@ namespace kaco {
 				{ "controlword_vl_rfg_enable",	static_cast<uint16_t>(1<<4) },
 				{ "controlword_vl_rfg_unlock",	static_cast<uint16_t>(1<<5) },
 				{ "controlword_vl_use_ref",		static_cast<uint16_t>(1<<6) },
-				
+
 				// control word flags profile position mode specific
 				{ "controlword_pp_new_set_point",				static_cast<uint16_t>(1<<4) },
 				{ "controlword_pp_change_set_immediately",		static_cast<uint16_t>(1<<5) },
 				{ "controlword_pp_abs_rel",						static_cast<uint16_t>(1<<6) },
-				
+
 				// control word flags homing mode specific
 				{ "controlword_hm_operation_start",	static_cast<uint16_t>(1<<4) },
-				
+
 				// control word flags interpolated position mode specific
 				{ "controlword_ip_enable_ip_mode",	static_cast<uint16_t>(1<<4) },
 
@@ -145,15 +161,15 @@ namespace kaco {
 				// status word flags profile position mode specific
 				{ "statusword_pp_set_point_acknowledge",	static_cast<uint16_t>(1<<12) },
 				{ "statusword_pp_following_error",			static_cast<uint16_t>(1<<13) },
-				
+
 				// status word flags profile velocity mode specific
 				{ "statusword_pv_speed",				static_cast<uint16_t>(1<<12) },
 				{ "statusword_pv_max_slippage_error",	static_cast<uint16_t>(1<<13) },
-				
+
 				// status word flags homing mode specific
 				{ "statusword_hm_homing_attained",	static_cast<uint16_t>(1<<12) },
 				{ "statusword_hm_homing_error",		static_cast<uint16_t>(1<<13) },
-				
+
 				// status word flags interpolated position mode specific
 				{ "statusword_ip_ip_mode_active",	static_cast<uint16_t>(1<<12) }
 			}

ros_bridge/include/bridge.h

@@ -28,7 +28,7 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
- 
+
 #pragma once
 
 #include "master.h"

ros_bridge/include/entry_publisher.h

@@ -28,7 +28,7 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
- 
+
 #pragma once
 
 #include "device.h"
@@ -37,20 +37,21 @@
 
 #include <string>
 
-namespace kaco {
+namespace kaco
+{
 
 	/// This class provides a Publisher implementation for
 	/// use with kaco::Bridge. It publishes a value from
 	/// a device's dictionary.
-	class EntryPublisher : public Publisher {
+	class EntryPublisher : public Publisher
+	{
 
 	public:
-
 		/// Constructor
 		/// \param device The CanOpen device
 		/// \param entry_name The name of the entry. See device profile.
 		/// \param access_method You can choose default/sdo/pdo method. See kaco::Device docs.
-		EntryPublisher(Device& device, const std::string& entry_name, const ReadAccessMethod access_method = ReadAccessMethod::use_default);
+		EntryPublisher(Device &device, const std::string &entry_name, const ReadAccessMethod access_method = ReadAccessMethod::use_default);
 
 		/// \see interface Publisher
 		void advertise() override;
@@ -59,7 +60,6 @@ namespace kaco {
 		void publish() override;
 
 	private:
-
 		static const bool debug = false;
 
 		// TODO: let the user change this?
@@ -69,11 +69,10 @@ namespace kaco {
 		std::string m_device_prefix;
 		std::string m_name;
 
-		Device& m_device;
+		Device &m_device;
 		std::string m_entry_name;
 		ReadAccessMethod m_access_method;
 		Type m_type;
-
 	};
 
 } // end namespace kaco

ros_bridge/include/entry_subscriber.h

@@ -28,7 +28,7 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
- 
+
 #pragma once
 
 #include "device.h"
@@ -43,38 +43,38 @@
 #include "std_msgs/Int32.h"
 #include "std_msgs/Bool.h"
 #include "std_msgs/String.h"
- 
+
 #include <string>
 #include <thread>
 
-namespace kaco {
+namespace kaco
+{
 
 	/// This class provides a Publisher implementation for
 	/// use with kaco::Bridge. It publishes a value from
 	/// a device's dictionary.
-	class EntrySubscriber : public Subscriber {
+	class EntrySubscriber : public Subscriber
+	{
 
 	public:
-
 		/// Constructor
 		/// \param device The CanOpen device
 		/// \param entry_name The name of the entry. See device profile.
 		/// \param access_method You can choose default/sdo/pdo method. See kaco::Device docs.
-		EntrySubscriber(Device& device, const std::string& entry_name, const WriteAccessMethod access_method = WriteAccessMethod::use_default);
+		EntrySubscriber(Device &device, const std::string &entry_name, const WriteAccessMethod access_method = WriteAccessMethod::use_default);
 
 		/// \see interface Subscriber
 		void advertise() override;
 
 	private:
-
-		void receive_uint8(const std_msgs::UInt8& msg);
-		void receive_uint16(const std_msgs::UInt16& msg);
-		void receive_uint32(const std_msgs::UInt32& msg);
-		void receive_int8(const std_msgs::Int8& msg);
-		void receive_int16(const std_msgs::Int16& msg);
-		void receive_int32(const std_msgs::Int32& msg);
-		void receive_boolean(const std_msgs::Bool& msg);
-		void receive_string(const std_msgs::String& msg);
+		void receive_uint8(const std_msgs::UInt8 &msg);
+		void receive_uint16(const std_msgs::UInt16 &msg);
+		void receive_uint32(const std_msgs::UInt32 &msg);
+		void receive_int8(const std_msgs::Int8 &msg);
+		void receive_int16(const std_msgs::Int16 &msg);
+		void receive_int32(const std_msgs::Int32 &msg);
+		void receive_boolean(const std_msgs::Bool &msg);
+		void receive_string(const std_msgs::String &msg);
 
 		static const bool debug = false;
 
@@ -85,11 +85,10 @@ namespace kaco {
 		std::string m_device_prefix;
 		std::string m_name;
 
-		Device& m_device;
+		Device &m_device;
 		std::string m_entry_name;
 		WriteAccessMethod m_access_method;
 		Type m_type;
-
 	};
 
 } // end namespace kaco

ros_bridge/include/joint_state_publisher.h

@@ -28,17 +28,18 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
- 
+
 #pragma once
 
 #include "device.h"
 #include "publisher.h"
 #include "ros/ros.h"
- 
+
 #include <string>
 #include <cmath>
 
-namespace kaco {
+namespace kaco
+{
 
 	/// This class provides a Publisher implementation for
 	/// use with kaco::Bridge and a CiA 402 motor device.
@@ -48,10 +49,10 @@ namespace kaco {
 	/// Currenty, only the motor angle is published.
 	/// You have to initialize the motor on your own.
 	/// The motor is expected to be in position mode.
-	class JointStatePublisher : public Publisher {
+	class JointStatePublisher : public Publisher
+	{
 
 	public:
-
 		/// Constructor
 		/// \param device a CiA 402 compliant motor device object
 		/// \param position_0_degree The motor position (dictionary
@@ -62,8 +63,15 @@ namespace kaco {
 		/// \param position_actual_field Name of the dictionary entry to use.
 		/// \param topic_name Custom topic name. Leave out for default.
 		/// \throws std::runtime_error if device is not CiA 402 compliant and in position_mode.
-		JointStatePublisher(Device& device, int32_t position_0_degree,
-			int32_t position_360_degree, const std::string& position_actual_field = "Position actual value", const std::string& topic_name = "");
+		JointStatePublisher(Device &device,
+							int32_t position_0_degree,
+							int32_t position_360_degree,
+							int32_t velocity_nominator,
+							int32_t velocity_denominator,
+							const std::string &position_actual_field = "Position actual value",
+							const std::string &velocity_actual_field = "Velocity actual value",
+							const std::string& torque_actual_field = "Torque actual value",
+							const std::string &topic_name = "");
 
 		/// \see interface Publisher
 		void advertise() override;
@@ -72,27 +80,36 @@ namespace kaco {
 		void publish() override;
 
 	private:
-
 		static const bool debug = false;
 
 		// TODO: let the user change this?
 		static const unsigned queue_size = 100;
 
-		/// converts "position actiual value" from CanOpen to radiant using m_position_0_degree and m_position_360_degree
-		double pos_to_rad(int32_t pos) const;
+		/// converts "position actual value" from CanOpen to revolutions using m_position_0_degree and m_position_360_degree
+		double pos_to_rev(int32_t pos) const;
+
+		/// converts "velocity actual value" from CanOpen to revolutions per second using m_velocity_nominator and m_velocity_denominator
+		double vel_to_rev_p_s(int32_t vel) const;
+
+		/// converts "torque actual value" from CanOpen to effort
+		double tor_to_eff(int32_t tor) const;
+
 
 		/// constant PI
 		static constexpr double pi() { return std::acos(-1); }
 
-		Device& m_device;
+		Device &m_device;
 		int32_t m_position_0_degree;
 		int32_t m_position_360_degree;
+		int32_t m_velocity_nominator;
+		int32_t m_velocity_denominator;
 		std::string m_position_actual_field;
+		std::string m_velocity_actual_field;
+		std::string m_torque_actual_field;
 		std::string m_topic_name;
 		bool m_initialized;
 
 		ros::Publisher m_publisher;
-
 	};
 
 } // end namespace kaco