Merge branch 'devel' into 'master'

v1.1

See merge request MAB_Robotics/ros2_mab_md80!8

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(ros2_mab_md80)
+project(candle_ros2)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
@@ -22,7 +22,7 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
-rosidl_generate_interfaces(ros2_mab_md80
+rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ImpedanceCommand.msg"
   "msg/MotionCommand.msg"
   "msg/Pid.msg"
@@ -36,14 +36,14 @@ rosidl_generate_interfaces(ros2_mab_md80
 )
 set(CMAKE_INSTALL_RPATH $ORIGIN)  # sets proper runtime paths for exec to link .so properly
 
-add_executable(md80_node src/md80_node.cpp)
-target_link_libraries(md80_node "${CMAKE_SOURCE_DIR}/lib/libcandle.so")
-target_include_directories(md80_node PUBLIC ${CMAKE_SOURCE_DIR}/include)
+add_executable(${PROJECT_NAME}_node src/md80_node.cpp)
+target_link_libraries(${PROJECT_NAME}_node "${CMAKE_SOURCE_DIR}/lib/libcandle.so")
+target_include_directories(${PROJECT_NAME}_node PUBLIC ${CMAKE_SOURCE_DIR}/include)
 
-rosidl_target_interfaces(md80_node ${PROJECT_NAME} "rosidl_typesupport_cpp")  # Links msgs from this very package
-ament_target_dependencies(md80_node rclcpp sensor_msgs)
+rosidl_target_interfaces(${PROJECT_NAME}_node ${PROJECT_NAME} "rosidl_typesupport_cpp")  # Links msgs from this very package
+ament_target_dependencies(${PROJECT_NAME}_node rclcpp sensor_msgs)
 
-install(TARGETS md80_node 
+install(TARGETS ${PROJECT_NAME}_node 
   DESTINATION lib/${PROJECT_NAME}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
   )

README.md

@@ -30,37 +30,37 @@ After starting the node, it will wait for configuration messages.
 Firstly, the node should be informed which drives should be present on the FDCAN bus. This can be done via `/add_md80s` service.
 For example:
 ```
-ros2 service call /md80_node/add_md80s ros2_mab_md80/srv/AddMd80s "drive_ids: [81, 97]"
+ros2 service call /candle_ros2_node/add_md80s candle_ros2/srv/AddMd80s "drive_ids: [81, 97]"
 ```
 Should produce the following output:
 ```
 waiting for service to become available...
-requester: making request: ros2_mab_md80.srv.AddMd80s_Request(drive_ids=[81, 97])
+requester: making request: candle_ros2.srv.AddMd80s_Request(drive_ids=[81, 97])
 
 response:
-ros2_mab_md80.srv.AddMd80s_Response(drives_success=[True, True], total_number_of_drives=2)
+candle_ros2.srv.AddMd80s_Response(drives_success=[True, True], total_number_of_drives=2)
 ```
 informing, that both drives (ids: 81 and 97), have been successfully contacted, and were added to the node's drives list.
 
 ### Set mode
 Next the desired control mode should be selected. This is accomplished with `/set_mode_md80s` service.
 For example:
 ```
-ros2 service call /md80_node/set_mode_md80s ros2_mab_md80/srv/SetModeMd80s "{drive_ids: [81, 97], mode:["IMPEDANCE", "IMPEDANCE"]}"
+ros2 service call /candle_ros2_node/set_mode_md80s candle_ros2/srv/SetModeMd80s "{drive_ids: [81, 97], mode:["IMPEDANCE", "IMPEDANCE"]}"
 ```
 Should produce:
 ```
-requester: making request: ros2_mab_md80.srv.SetModeMd80s_Request(drive_ids=[81, 97], mode=['IMPEDANCE', 'IMPEDANCE'])
+requester: making request: candle_ros2.srv.SetModeMd80s_Request(drive_ids=[81, 97], mode=['IMPEDANCE', 'IMPEDANCE'])
 
 response:
-ros2_mab_md80.srv.SetModeMd80s_Response(drives_success=[True, True])
+candle_ros2.srv.SetModeMd80s_Response(drives_success=[True, True])
 ```
 Informing that for both drives mode has been set correctly.
 
 ### Set Zero 
 Often when starting, setting a current position to zero is desired. This can be accomplished with a call to `/zero_md80s` service.
 ```
-ros2 service call /md80_node/zero_md80s ros2_mab_md80/srv/GenericMd80Msg "{drive_ids:[81, 97]}"
+ros2 service call /candle_ros2_node/zero_md80s candle_ros2/srv/GenericMd80Msg "{drive_ids:[81, 97]}"
 ```
 
 ### Enabling/Disabling drives
@@ -71,13 +71,19 @@ After enabling, the node will publish current joint states to `/md80/joint_state
 
 The node will also listen for the messages on topics for controlling the drives. All of the above topics are listened to all the time, but currently applied settings are dependent on the md80 mode set before enabling.
 
+```
+ros2 service call /candle_ros2_node/enable_md80s candle_ros2/srv/GenericMd80Msg "{drive_ids:[81, 97]}"
+
+ros2 service call /candle_ros2_node/disable_md80s candle_ros2/srv/GenericMd80Msg "{drive_ids:[81, 97]}"
+```
+
 ### Controlling drives
 Controlling the drives is done via the four topics listed above. For commands to be viable, all fields of each message must be filled properly. For example, to set up custom gains for IMPEDANCE mode use:
 ```
-ros2 topic pub /md80/impedance_command ros2_mab_md80/msg/ImpedanceCommand "{drive_ids:[81, 97], kp:[0.25, 1.0], kd:[0.1, 0.05], max_output:[2.0, 2.0]}"
+ros2 topic pub /md80/impedance_command candle_ros2/msg/ImpedanceCommand "{drive_ids:[81, 97], kp:[0.25, 1.0], kd:[0.1, 0.05], max_output:[2.0, 2.0]}"
 ```
 
 Setting desired position, velocity, and torque is done via `/md80/motion_command` topic. Note that for it to take effect, all fields in the message should be correctly filled. For example, to move the drives in impedance mode, it is possible to use the following command
 ```
-ros2 topic pub /md80/motion_command ros2_mab_md80/msg/MotionCommand "{drive_ids:[81,97], target_position:[3.0, -3.0], target_velocity:[0.0, 0.0], target_torque:[0, 0]}" -1
+ros2 topic pub /md80/motion_command candle_ros2/msg/MotionCommand "{drive_ids:[81,97], target_position:[3.0, -3.0], target_velocity:[0.0, 0.0], target_torque:[0, 0]}" -1
 ```

include/candle.hpp → include/Candle/candle.hpp

@@ -39,7 +39,7 @@ namespace mab
     class Candle
     {
     private:
-        const std::string version = "v2.1";
+        const std::string version = "v2.2";
         UsbDevice*usb;
         std::thread receiverThread;
         std::thread transmitterThread;

include/md80_node.hpp

@@ -1,14 +1,14 @@
-#include "candle.hpp"
+#include "Candle/candle.hpp"
 
-#include "ros2_mab_md80/msg/impedance_command.hpp"
-#include "ros2_mab_md80/msg/motion_command.hpp"
-#include "ros2_mab_md80/msg/velocity_pid_command.hpp"
-#include "ros2_mab_md80/msg/position_pid_command.hpp"
+#include "candle_ros2/msg/impedance_command.hpp"
+#include "candle_ros2/msg/motion_command.hpp"
+#include "candle_ros2/msg/velocity_pid_command.hpp"
+#include "candle_ros2/msg/position_pid_command.hpp"
 
-#include "ros2_mab_md80/srv/add_md80s.hpp"
-#include "ros2_mab_md80/srv/generic_md80_msg.hpp"
-#include "ros2_mab_md80/srv/set_mode_md80s.hpp"
-#include "ros2_mab_md80/srv/set_limits_md80.hpp"
+#include "candle_ros2/srv/add_md80s.hpp"
+#include "candle_ros2/srv/generic_md80_msg.hpp"
+#include "candle_ros2/srv/set_mode_md80s.hpp"
+#include "candle_ros2/srv/set_limits_md80.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
@@ -24,33 +24,33 @@ class Md80Node : public rclcpp::Node
 
     rclcpp::TimerBase::SharedPtr pubTimer;
 
-    rclcpp::Service<ros2_mab_md80::srv::AddMd80s>::SharedPtr addMd80Service;
-    rclcpp::Service<ros2_mab_md80::srv::GenericMd80Msg>::SharedPtr zeroMd80Service;
-    rclcpp::Service<ros2_mab_md80::srv::SetModeMd80s>::SharedPtr setModeMd80Service;
-    rclcpp::Service<ros2_mab_md80::srv::GenericMd80Msg>::SharedPtr enableMd80Service;
-    rclcpp::Service<ros2_mab_md80::srv::GenericMd80Msg>::SharedPtr disableMd80Service;
-    rclcpp::Service<ros2_mab_md80::srv::SetLimitsMd80>::SharedPtr setLimitsMd80Service;
+    rclcpp::Service<candle_ros2::srv::AddMd80s>::SharedPtr addMd80Service;
+    rclcpp::Service<candle_ros2::srv::GenericMd80Msg>::SharedPtr zeroMd80Service;
+    rclcpp::Service<candle_ros2::srv::SetModeMd80s>::SharedPtr setModeMd80Service;
+    rclcpp::Service<candle_ros2::srv::GenericMd80Msg>::SharedPtr enableMd80Service;
+    rclcpp::Service<candle_ros2::srv::GenericMd80Msg>::SharedPtr disableMd80Service;
+    rclcpp::Service<candle_ros2::srv::SetLimitsMd80>::SharedPtr setLimitsMd80Service;
 
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr jointStatePub;
-    rclcpp::Subscription<ros2_mab_md80::msg::MotionCommand>::SharedPtr motionCommandSub;
-    rclcpp::Subscription<ros2_mab_md80::msg::ImpedanceCommand>::SharedPtr impedanceCommandSub;
-    rclcpp::Subscription<ros2_mab_md80::msg::VelocityPidCommand>::SharedPtr velocityCommandSub;
-    rclcpp::Subscription<ros2_mab_md80::msg::PositionPidCommand>::SharedPtr positionCommandSub;
+    rclcpp::Subscription<candle_ros2::msg::MotionCommand>::SharedPtr motionCommandSub;
+    rclcpp::Subscription<candle_ros2::msg::ImpedanceCommand>::SharedPtr impedanceCommandSub;
+    rclcpp::Subscription<candle_ros2::msg::VelocityPidCommand>::SharedPtr velocityCommandSub;
+    rclcpp::Subscription<candle_ros2::msg::PositionPidCommand>::SharedPtr positionCommandSub;
 
-    void service_addMd80(const std::shared_ptr<ros2_mab_md80::srv::AddMd80s::Request> request,
-        std::shared_ptr<ros2_mab_md80::srv::AddMd80s::Response> response);
-    void service_zeroMd80(const std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Request> request,
-        std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Response> response);
-    void service_setModeMd80(const std::shared_ptr<ros2_mab_md80::srv::SetModeMd80s::Request> request,
-        std::shared_ptr<ros2_mab_md80::srv::SetModeMd80s::Response> response);
-    void service_enableMd80(const std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Request> request,
-        std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Response> response);
-    void service_disableMd80(const std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Request> request,
-        std::shared_ptr<ros2_mab_md80::srv::GenericMd80Msg::Response> response);
+    void service_addMd80(const std::shared_ptr<candle_ros2::srv::AddMd80s::Request> request,
+        std::shared_ptr<candle_ros2::srv::AddMd80s::Response> response);
+    void service_zeroMd80(const std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Request> request,
+        std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Response> response);
+    void service_setModeMd80(const std::shared_ptr<candle_ros2::srv::SetModeMd80s::Request> request,
+        std::shared_ptr<candle_ros2::srv::SetModeMd80s::Response> response);
+    void service_enableMd80(const std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Request> request,
+        std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Response> response);
+    void service_disableMd80(const std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Request> request,
+        std::shared_ptr<candle_ros2::srv::GenericMd80Msg::Response> response);
 
     void publishJointStates();
-    void motionCommandCallback(const std::shared_ptr<ros2_mab_md80::msg::MotionCommand> msg);
-    void impedanceCommandCallback(const std::shared_ptr<ros2_mab_md80::msg::ImpedanceCommand> msg);
-    void velocityCommandCallback(const std::shared_ptr<ros2_mab_md80::msg::VelocityPidCommand> msg);
-    void positionCommandCallback(const std::shared_ptr<ros2_mab_md80::msg::PositionPidCommand> msg);
+    void motionCommandCallback(const std::shared_ptr<candle_ros2::msg::MotionCommand> msg);
+    void impedanceCommandCallback(const std::shared_ptr<candle_ros2::msg::ImpedanceCommand> msg);
+    void velocityCommandCallback(const std::shared_ptr<candle_ros2::msg::VelocityPidCommand> msg);
+    void positionCommandCallback(const std::shared_ptr<candle_ros2::msg::PositionPidCommand> msg);
 };

msg/PositionPidCommand.msg

@@ -1,3 +1,3 @@
 uint16[] drive_ids
-ros2_mab_md80/Pid[] position_pid
-ros2_mab_md80/Pid[] velocity_pid
+candle_ros2/Pid[] position_pid
+candle_ros2/Pid[] velocity_pid

msg/VelocityPidCommand.msg

@@ -1,2 +1,2 @@
 uint16[] drive_ids
-ros2_mab_md80/Pid[] velocity_pid
+candle_ros2/Pid[] velocity_pid

package.xml

@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>ros2_mab_md80</name>
-  <version>0.0.1</version>
-  <description>TODO: Package description</description>
-  <maintainer email="jakub.matyszczak@mabrobotics.pl">jmatyszczak</maintainer>
-  <license>TODO: License declaration</license>
+  <name>candle_ros2</name>
+  <version>1.1.0</version>
+  <description>ROS2 node for interacting with MAB md80 drives using CANdle library.</description>
+  <maintainer email="github@mabrobotics.pl">MAB Robotics</maintainer>
+  <license>GNU Lesser General Public License</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>