Merge pull request #37 from UM-ARM-Lab/merge_control_mode_status_and_…

…command

Merge control mode status and command

victor_hardware_interface/CMakeLists.txt

@@ -4,16 +4,16 @@ project(victor_hardware_interface)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  arc_utilities
-  diagnostic_msgs
-  geometry_msgs
-  message_generation
-  roscpp
-  sensor_msgs
-  std_msgs
-  lcm
-)
+set(CATKIN_PACKAGES
+    arc_utilities
+    diagnostic_msgs
+    geometry_msgs
+    roscpp
+    sensor_msgs
+    std_msgs
+    lcm)
+
+find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGES} message_generation)
 find_package(cmake_modules REQUIRED)
 find_package(Eigen3 REQUIRED)
 set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
@@ -52,25 +52,28 @@ set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-add_message_files(DIRECTORY msg FILES CartesianImpedanceParameters.msg
-                                      CartesianControlModeLimits.msg
-                                      MotionStatus.msg
-                                      CartesianValueQuantity.msg
-                                      JointPathExecutionParameters.msg
-                                      CartesianPathExecutionParameters.msg
-                                      ControlModeCommand.msg
-                                      Robotiq3FingerActuatorCommand.msg
-                                      ControlModeStatus.msg
-                                      Robotiq3FingerActuatorStatus.msg
-                                      JointImpedanceParameters.msg
-                                      Robotiq3FingerCommand.msg
-                                      JointValueQuantity.msg
-                                      Robotiq3FingerObjectStatus.msg
-                                      MotionCommand.msg
-                                      Robotiq3FingerStatus.msg)
+add_message_files(DIRECTORY msg FILES
+    CartesianControlModeLimits.msg
+    CartesianImpedanceParameters.msg
+    CartesianPathExecutionParameters.msg
+    CartesianValueQuantity.msg
+    ControlMode.msg
+    ControlModeParameters.msg
+    JointImpedanceParameters.msg
+    JointPathExecutionParameters.msg
+    JointValueQuantity.msg
+    MotionCommand.msg
+    MotionStatus.msg
+    Robotiq3FingerActuatorCommand.msg
+    Robotiq3FingerActuatorStatus.msg
+    Robotiq3FingerCommand.msg
+    Robotiq3FingerObjectStatus.msg
+    Robotiq3FingerStatus.msg)
 
 ## Generate services in the 'srv' folder
-add_service_files(DIRECTORY srv FILES SetControlMode.srv GetControlMode.srv)
+add_service_files(DIRECTORY srv FILES
+    SetControlMode.srv
+    GetControlMode.srv)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -80,7 +83,11 @@ add_service_files(DIRECTORY srv FILES SetControlMode.srv GetControlMode.srv)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages(DEPENDENCIES diagnostic_msgs geometry_msgs sensor_msgs std_msgs)
+generate_messages(DEPENDENCIES
+    diagnostic_msgs
+    geometry_msgs
+    sensor_msgs
+    std_msgs)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -114,15 +121,8 @@ generate_messages(DEPENDENCIES diagnostic_msgs geometry_msgs sensor_msgs std_msg
 catkin_package(
     INCLUDE_DIRS    include 
     LIBRARIES       ${PROJECT_NAME} 
-    CATKIN_DEPENDS  arc_utilities 
-                    diagnostic_msgs 
-                    geometry_msgs 
-                    message_runtime 
-                    roscpp 
-                    sensor_msgs 
-                    std_msgs 
-                    lcm 
-    DEPENDS         Eigen3)
+    CATKIN_DEPENDS  ${CATKIN_PACKAGES}
+                    message_runtime)
 
 ###########
 ## Build ##
@@ -137,16 +137,17 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror -Wconversion")
 set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -Wconversion -O3 -g -flto")
 
 ## Declare C++ executables
-add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iiwa_hardware_interface.hpp
-                            include/${PROJECT_NAME}/robotiq_3finger_hardware_interface.hpp
-                            include/${PROJECT_NAME}/wrist_force_torque_hardware_interface.hpp
-                            include/${PROJECT_NAME}/takktile_hardware_interface.hpp
-                            include/${PROJECT_NAME}/minimal_arm_wrapper_interface.hpp
-                            src/${PROJECT_NAME}/iiwa_hardware_interface.cpp
-                            src/${PROJECT_NAME}/robotiq_3finger_hardware_interface.cpp
-                            src/${PROJECT_NAME}/wrist_force_torque_hardware_interface.cpp
-                            src/${PROJECT_NAME}/takktile_hardware_interface.cpp
-                            src/${PROJECT_NAME}/minimal_arm_wrapper_interface.cpp)
+add_library(${PROJECT_NAME}
+    include/${PROJECT_NAME}/iiwa_hardware_interface.hpp
+    include/${PROJECT_NAME}/robotiq_3finger_hardware_interface.hpp
+    include/${PROJECT_NAME}/wrist_force_torque_hardware_interface.hpp
+    include/${PROJECT_NAME}/takktile_hardware_interface.hpp
+    include/${PROJECT_NAME}/minimal_arm_wrapper_interface.hpp
+    src/${PROJECT_NAME}/iiwa_hardware_interface.cpp
+    src/${PROJECT_NAME}/robotiq_3finger_hardware_interface.cpp
+    src/${PROJECT_NAME}/wrist_force_torque_hardware_interface.cpp
+    src/${PROJECT_NAME}/takktile_hardware_interface.cpp
+    src/${PROJECT_NAME}/minimal_arm_wrapper_interface.cpp)
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 

victor_hardware_interface/include/victor_hardware_interface/iiwa_hardware_interface.hpp

@@ -1,13 +1,11 @@
 #include <string>
 #include <lcm/lcm-cpp.hpp>
 // ROS message headers
-#include "victor_hardware_interface/ControlModeCommand.h"
-#include "victor_hardware_interface/ControlModeStatus.h"
+#include "victor_hardware_interface/ControlModeParameters.h"
 #include "victor_hardware_interface/MotionCommand.h"
 #include "victor_hardware_interface/MotionStatus.h"
 // LCM type headers
-#include "victor_hardware_interface/control_mode_command.hpp"
-#include "victor_hardware_interface/control_mode_status.hpp"
+#include "victor_hardware_interface/control_mode_parameters.hpp"
 #include "victor_hardware_interface/motion_command.hpp"
 #include "victor_hardware_interface/motion_status.hpp"
 
@@ -44,11 +42,14 @@ namespace victor_hardware_interface
     CartesianControlModeLimits cartesianControlModeLimitsLcmToRos(const cartesian_control_mode_limits& lcm_ccml);
     cartesian_control_mode_limits cartesianControlModeLimitsRosToLcm(const CartesianControlModeLimits& ros_ccml);
 
+    ControlMode controlModeLcmToRos(const control_mode& lcm_cm);
+    control_mode controlModeRosToLcm(const ControlMode& ros_cm);
+
     MotionStatus motionStatusLcmToRos(const motion_status& lcm_status);
     motion_command motionCommandRosToLcm(const MotionCommand& ros_command);
 
-    ControlModeStatus controlModeStatusLcmToRos(const control_mode_status& lcm_status);
-    control_mode_command controlModeCommandRosToLcm(const ControlModeCommand& ros_command);
+    ControlModeParameters controlModeParamsLcmToRos(const control_mode_parameters& lcm_cmp);
+    control_mode_parameters controlModeParamsRosToLcm(const ControlModeParameters& ros_cmp);
 
     ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
     // The class that does the actual communication
@@ -65,7 +66,7 @@ namespace victor_hardware_interface
         std::function<void(const MotionStatus&)> motion_status_callback_fn_;
         std::string control_mode_command_channel_name_;
         std::string control_mode_status_channel_name_;
-        std::function<void(const ControlModeStatus&)> control_mode_status_callback_fn_;
+        std::function<void(const ControlModeParameters&)> control_mode_status_callback_fn_;
 
         void InternalMotionStatusLCMCallback(
                 const lcm::ReceiveBuffer* buffer,
@@ -75,7 +76,7 @@ namespace victor_hardware_interface
         void InternalControlModeStatusLCMCallback(
                 const lcm::ReceiveBuffer* buffer,
                 const std::string& channel,
-                const control_mode_status* status_msg);
+                const control_mode_parameters* status_msg);
 
     public:
 
@@ -87,11 +88,11 @@ namespace victor_hardware_interface
                 const std::function<void(const MotionStatus&)>& motion_status_callback_fn,
                 const std::string& control_mode_command_channel_name,
                 const std::string& control_mode_status_channel_name,
-                const std::function<void(const ControlModeStatus&)>& control_mode_status_callback_fn);
+                const std::function<void(const ControlModeParameters&)>& control_mode_status_callback_fn);
 
         bool SendMotionCommandMessage(const MotionCommand& command);
 
-        bool SendControlModeCommandMessage(const ControlModeCommand& command);
+        bool SendControlModeCommandMessage(const ControlModeParameters& command);
     };
 }
 

victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp

@@ -7,8 +7,7 @@
 #include <ros/ros.h>
 #include <ros/callback_queue.h>
 // ROS message headers
-#include <victor_hardware_interface/ControlModeCommand.h>
-#include <victor_hardware_interface/ControlModeStatus.h>
+#include <victor_hardware_interface/ControlModeParameters.h>
 #include <victor_hardware_interface/MotionCommand.h>
 #include <victor_hardware_interface/MotionStatus.h>
 #include <victor_hardware_interface/Robotiq3FingerCommand.h>
@@ -40,7 +39,7 @@ namespace victor_hardware_interface
 
     bool cartesianPexpEqual(const CartesianPathExecutionParameters& pexp1, const CartesianPathExecutionParameters& pexp2);
 
-    bool controlModeCommandAndStatusEqual(const ControlModeCommand& command, const ControlModeStatus& status);
+    bool controlModeParamsEqual(const ControlModeParameters& params1, const ControlModeParameters& params2);
 
 
     /**
@@ -92,7 +91,7 @@ namespace victor_hardware_interface
         ros::CallbackQueue ros_callback_queue_;
 
         mutable std::mutex control_mode_status_mutex_;
-        Maybe::Maybe<ControlModeStatus> active_control_mode_;
+        Maybe::Maybe<ControlModeParameters> active_control_mode_;
         const double set_control_mode_timeout_; // measured in seconds
 
         std::shared_ptr<lcm::LCM> send_lcm_ptr_;
@@ -120,7 +119,7 @@ namespace victor_hardware_interface
 
         static std::pair<bool, std::string> validateCartesianControlModeLimits(const CartesianControlModeLimits& params);
 
-        static std::pair<bool, std::string> validateControlMode(const ControlModeCommand& control_mode);
+        static std::pair<bool, std::string> validateControlMode(const ControlModeParameters& params);
 
         //// ROS Callbacks to get and set the control mode as service calls ////////////////////////////////////////////
 
@@ -132,7 +131,7 @@ namespace victor_hardware_interface
          * Callback function used by the LCM subsystem when a control_mode_status message is received. Caches the value
          * in active_control_mode_ for use by setControlModeCallback(...) and getControlModeCallback(...)
          */
-        void controlModeStatusLCMCallback(const ControlModeStatus& control_mode_status);
+        void controlModeStatusLCMCallback(const ControlModeParameters& control_mode_status);
 
         ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
         // Arm movement/control and feedback functionality

victor_hardware_interface/lcm_defs/control_mode.lcm

@@ -0,0 +1,13 @@
+package victor_hardware_interface;
+
+struct control_mode
+{
+    const int8_t IS_POSITION_MOTION=0; // Flag to bitmask
+    const int8_t IS_CARTESIAN_MOTION=1; // Flag to bitmask
+    const int8_t IS_IMPEDANCE_CONTROL=2; // Flag to bitmask
+    const int8_t JOINT_POSITION=0;
+    const int8_t JOINT_IMPEDANCE=2;
+    const int8_t CARTESIAN_POSE=1;
+    const int8_t CARTESIAN_IMPEDANCE=3;
+    int8_t mode;
+}

victor_hardware_interface/lcm_defs/control_mode_command.lcm → victor_hardware_interface/lcm_defs/control_mode_parameters.lcm

@@ -1,19 +1,12 @@
 package victor_hardware_interface;
 
-struct control_mode_command
+struct control_mode_parameters
 {
     victor_hardware_interface.joint_impedance_parameters joint_impedance_params;
     victor_hardware_interface.cartesian_impedance_parameters cartesian_impedance_params;
     victor_hardware_interface.cartesian_control_mode_limits cartesian_control_mode_limits;
     victor_hardware_interface.joint_path_execution_parameters joint_path_execution_params;
     victor_hardware_interface.cartesian_path_execution_parameters cartesian_path_execution_params;
+    victor_hardware_interface.control_mode control_mode;
     double timestamp;
-    const int8_t IS_POSITION_MOTION=0; // Flag to bitmask
-    const int8_t IS_CARTESIAN_MOTION=1; // Flag to bitmask
-    const int8_t IS_IMPEDANCE_CONTROL=2; // Flag to bitmask
-    const int8_t JOINT_POSITION=0;
-    const int8_t JOINT_IMPEDANCE=2;
-    const int8_t CARTESIAN_POSE=1;
-    const int8_t CARTESIAN_IMPEDANCE=3;
-    int8_t control_mode;
 }

victor_hardware_interface/lcm_defs/make_defs.sh

@@ -3,9 +3,9 @@
 for lcmdef in *.lcm
 do
     echo "Generating C++11 type for LCM def $lcmdef"
-    ~/lcm/lcmgen/lcm-gen -x --cpp-std c++11 $lcmdef
+    lcm-gen -x --cpp-std c++11 $lcmdef
     echo "Generating Java type for LCM def $lcmdef"
-    ~/lcm/lcmgen/lcm-gen -j $lcmdef
+    lcm-gen -j $lcmdef
 done
 
 echo "Removing previously generated types"

victor_hardware_interface/lcm_defs/motion_command.lcm

@@ -5,13 +5,6 @@ struct motion_command
     victor_hardware_interface.joint_value_quantity joint_position;
     victor_hardware_interface.joint_value_quantity joint_velocity;
     victor_hardware_interface.cartesian_pose cartesian_pose;
+    victor_hardware_interface.control_mode control_mode;
     double timestamp;
-    const int8_t IS_POSITION_MOTION=0; // Flag to bitmask
-    const int8_t IS_CARTESIAN_MOTION=1; // Flag to bitmask
-    const int8_t IS_IMPEDANCE_CONTROL=2; // Flag to bitmask
-    const int8_t JOINT_POSITION=0;
-    const int8_t JOINT_IMPEDANCE=2;
-    const int8_t CARTESIAN_POSE=1;
-    const int8_t CARTESIAN_IMPEDANCE=3;
-    int8_t control_mode;
 }

victor_hardware_interface/lcm_defs/motion_status.lcm

@@ -7,18 +7,11 @@ struct motion_status
     victor_hardware_interface.joint_value_quantity measured_joint_velocity;
     victor_hardware_interface.joint_value_quantity measured_joint_torque;
     victor_hardware_interface.joint_value_quantity estimated_external_torque;
+    victor_hardware_interface.cartesian_value_quantity estimated_external_wrench;
     victor_hardware_interface.cartesian_value_quantity measured_cartesian_pose_raw;
     victor_hardware_interface.cartesian_value_quantity commanded_cartesian_pose_raw;
     victor_hardware_interface.cartesian_pose measured_cartesian_pose;
     victor_hardware_interface.cartesian_pose commanded_cartesian_pose;
-    victor_hardware_interface.cartesian_value_quantity estimated_external_wrench;
+    victor_hardware_interface.control_mode active_control_mode;
     double timestamp;
-    const int8_t IS_POSITION_MOTION=0; // Flag to bitmask
-    const int8_t IS_CARTESIAN_MOTION=1; // Flag to bitmask
-    const int8_t IS_IMPEDANCE_CONTROL=2; // Flag to bitmask
-    const int8_t JOINT_POSITION=0;
-    const int8_t JOINT_IMPEDANCE=2;
-    const int8_t CARTESIAN_POSE=1;
-    const int8_t CARTESIAN_IMPEDANCE=3;
-    int8_t active_control_mode;
 }