setup new hw controller

IvanSnap360 · Jun 14, 2023 · a3c27fc · a3c27fc
1 parent dd99496
commit a3c27fc
Show file tree

Hide file tree

Showing 4 changed files with 80 additions and 19 deletions.
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
@@ -128,7 +128,7 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES omnibot_manipulator_hw_controller_lib
- CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib
+ CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib trajectory_msgs dynamixel_workbench_msgs dynamixel_workbench_toolbox cmake_modules
 #  DEPENDS system_lib 
 )
 
@@ -141,12 +141,14 @@ catkin_package(
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIRS}
+
 )
 
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/omnibot_manipulator_hw_controller.cpp
-# )
+# Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/${PROJECT_NAME}_lib/omnibot_manipulator_hw_controller_lib.cpp
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -156,7 +158,8 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.cpp)
+add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.cpp 
+                                    src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -171,10 +174,9 @@ add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.c
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}
+  yaml-cpp
 )
-# target_link_libraries(action_server
-#   ${catkin_LIBRARIES}
-# )
+
 
 #############
 ## Install ##

diff --git a/...ler/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h b/...ler/include/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h
@@ -1,12 +1,48 @@
 #ifndef __OMNIBOT_MANIPULATOR_HW_CONTROLLER_H__
 #define __OMNIBOT_MANIPULATOR_HW_CONTROLLER_H__
 #include <dynamixel_workbench_toolbox/dynamixel_workbench.h>
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/FollowJointTrajectoryGoal.h>
+#include <control_msgs/FollowJointTrajectoryResult.h>
+#include <control_msgs/FollowJointTrajectoryFeedback.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include "yaml-cpp/yaml.h"
+#include <sensor_msgs/JointState.h>
 class OMNIBOT_MANIPULATOR_HW_CONTROLLER
 {
 private:
-
+    YAML::Node _cfg;
+    ros::NodeHandle *_nh;
+    DynamixelWorkbench *_dxl;
+
+    std::string _port_path;
+    uint32_t _baudrate;
+
+    std::string _manipulator_action_server_name;
+    std::string _gripper_action_server_name;
+    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> *_manipulator_action_server;
+    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> *_gripper_action_server;
+
+    void _manipulator_action_server_goal_cb_f();
+    void _manipulator_action_server_preempt_cb_f();
+    void _gripper_action_server_goal_cb_f();
+    void _gripper_action_server_preempt_cb_f();
+
+    ros::Publisher _normalized_manipulator_joint_state_data_publisher;
+    ros::Subscriber _raw_manipulator_joint_state_data_subscriber;
+    void _raw_manipulator_joint_state_data_subscriber_cb_f(const sensor_msgs::JointState::ConstPtr &in_data);
+
+
+    void readConfig(std::string path);
+    void initPublishers();
+    void initSubscribers();
+    void initActions();
+    void initServices();
 public:
-    OMNIBOT_MANIPULATOR_HW_CONTROLLER();
+    OMNIBOT_MANIPULATOR_HW_CONTROLLER(ros::NodeHandle *nh, std::string config_path);
     ~OMNIBOT_MANIPULATOR_HW_CONTROLLER();
 };
 

diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
@@ -49,23 +49,45 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>dynamixel_workbench_msgs</build_depend>
+  <build_depend>dynamixel_workbench_toolbox</build_depend>
+  <build_depend>cmake_modules</build_depend>
+
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>trajectory_msgs</build_export_depend>
+  <build_export_depend>dynamixel_workbench_msgs</build_export_depend>
+  <build_export_depend>dynamixel_workbench_toolbox</build_export_depend>
+  <build_export_depend>cmake_modules</build_export_depend>
+
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
-  <exec_depend>message_generation</exec_depend>
-  <exec_depend>actionlib_msgs</exec_depend>
-  <build_export_depend>actionlib_msgs</build_export_depend>
-  <build_depend>actionlib_msgs</build_depend>
-  <build_depend>actionlib</build_depend>
-  <build_export_depend>actionlib</build_export_depend>
-  <exec_depend>actionlib</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>dynamixel_workbench_msgs</exec_depend>
+  <exec_depend>dynamixel_workbench_toolbox</exec_depend>
+  <exec_depend>cmake_modules</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

diff --git a/...oller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.cpp b/...oller/src/omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.cpp
@@ -0,0 +1 @@
+#include "omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h"
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		#include "omnibot_manipulator_hw_controller_lib/omnibot_manipulator_hw_controller_lib.h"