Add joint limits and cpp autoformat (#129)

* autoformat using armlab .clang file

* Add .clang-format and autoformat several files

* Add proper joint limits for victor

* Fix some fat-fingered numbers

victor_hardware_interface/.clang-format

@@ -0,0 +1,168 @@
+---
+Language: Cpp
+# BasedOnStyle:  Google
+AccessModifierOffset: -1
+AlignAfterOpenBracket: Align
+AlignConsecutiveMacros: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Left
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: WithoutElse
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: false
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterObjCDeclaration: false
+  AfterStruct: false
+  AfterUnion: false
+  AfterExternBlock: false
+  BeforeCatch: false
+  BeforeElse: false
+  IndentBraces: false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit: 120
+CommentPragmas: '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DeriveLineEnding: true
+DerivePointerAlignment: true
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks: Regroup
+IncludeCategories:
+  - Regex: '^<ext/.*\.h>'
+    Priority: 2
+    SortPriority: 0
+  - Regex: '^<.*\.h>'
+    Priority: 1
+    SortPriority: 0
+  - Regex: '^<.*'
+    Priority: 2
+    SortPriority: 0
+  - Regex: '.*'
+    Priority: 3
+    SortPriority: 0
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
+IndentCaseLabels: true
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentWidth: 2
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+RawStringFormats:
+  - Language: Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle: google
+  - Language: TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+    CanonicalDelimiter: ''
+    BasedOnStyle: google
+ReflowComments: true
+SortIncludes: true
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
+Standard: Auto
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth: 8
+UseCRLF: false
+UseTab: Never
+...
+

victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp

@@ -53,9 +53,9 @@ namespace victor_hardware_interface
 
         MinimalArmWrapperInterface(
                 ros::NodeHandle& nh,
-                const std::shared_ptr<lcm::LCM>& send_lcm_ptr,
-                const std::shared_ptr<lcm::LCM>& recv_lcm_ptr,
-                const std::string& cartesian_control_frame,
+                std::shared_ptr<lcm::LCM>  send_lcm_ptr,
+                std::shared_ptr<lcm::LCM>  recv_lcm_ptr,
+                std::string  cartesian_control_frame,
                 const double set_control_mode_timeout,
                 // ROS Topics
                 const std::string& motion_command_topic,

victor_hardware_interface/scripts/manual_motion.py

@@ -16,15 +16,15 @@
 from victor_hardware_interface_msgs.msg import MotionCommand, MotionStatus, ControlModeParameters, ControlMode
 from threading import Lock
 from arc_utilities import ros_helpers as rh
-
+from arc_utilities.listener import Listener
 
 joint_names = ['joint_' + str(i) for i in range(1, 8)]
 
-hard_limits_deg = np.array([170.0,  120.0,  170.0,  120.0,  170.0,  120.0,  175.0])
+hard_limits_deg = np.array([170.0, 120.0, 170.0, 120.0, 170.0, 120.0, 175.0])
 safety_deg = 30.0  # Stay far away from joint hard limits
 
-joint_lower = -(hard_limits_deg - safety_deg) * np.pi/180
-joint_upper =  (hard_limits_deg - safety_deg) * np.pi/180
+joint_lower = -(hard_limits_deg - safety_deg) * np.pi / 180
+joint_upper = (hard_limits_deg - safety_deg) * np.pi / 180
 
 
 class ManualMotion:
@@ -35,7 +35,7 @@ def __init__(self, arm_name):
 
         self.threshold = 0.05
         self.arrive_threshold = 0.02
-        
+
         self.low_pass_tau = 0.01  # seconds
         self.prev_time = None
         self.lock = Lock()
@@ -47,7 +47,7 @@ def callback_update(self, msg):
             self.run_lowpass(msg)
 
     def run_lowpass(self, msg):
-        
+
         if self.prev_time is None:
             self.prev_time = time.time()
 
@@ -61,12 +61,11 @@ def run_lowpass(self, msg):
             assert False
 
         # Update our internal copy and calculate error for later thresholding
-        meas = np.array([getattr(msg.measured_joint_position,  joint) for joint in joint_names])
-        cmd =  np.array([getattr(msg.commanded_joint_position, joint) for joint in joint_names])
+        meas = np.array([getattr(msg.measured_joint_position, joint) for joint in joint_names])
+        cmd = np.array([getattr(msg.commanded_joint_position, joint) for joint in joint_names])
         err = np.linalg.norm(meas - cmd)
         self.last_pos = meas
 
-
         if err > self.threshold:
             # Robot is far enough away from setpoint that we assume someone is pushing it
             self.follow = True
@@ -92,8 +91,8 @@ def run_lowpass(self, msg):
         for idx in range(len(joint_names)):
             setattr(cmd_msg.joint_position, joint_names[idx], new_cmd[idx])
         self.pub.publish(cmd_msg)
-        
-        
+
+
 def print_joints(left, right):
     """Print nicely to the terminal so joint values can be copied"""
     rospy.loginfo("Joint angles are: ")
@@ -106,33 +105,30 @@ def print_joints(left, right):
 
 if __name__ == "__main__":
     rospy.init_node("manual_motion")
-
 
     control_mode_params = vu.get_joint_impedance_params(vu.Stiffness.MEDIUM)
 
     use_left_arm = rospy.get_param("~use_left_arm", True)
     use_right_arm = rospy.get_param("~use_right_arm", True)
 
-
-
-    if(use_left_arm):
+    if use_left_arm:
         print("initializing left arm ...")
         sys.stdout.flush()
-        l_cm = rh.Listener("left_arm/control_mode_status", ControlModeParameters)
+        l_cm = Listener("left_arm/control_mode_status", ControlModeParameters)
         cur_mode = l_cm.get(block_until_data=True)
         control_mode_params.joint_path_execution_params.joint_relative_velocity = cur_mode.joint_path_execution_params.joint_relative_velocity
         control_mode_params.joint_path_execution_params.joint_relative_acceleration = cur_mode.joint_path_execution_params.joint_relative_acceleration
-        
+
         result = vu.send_new_control_mode("left_arm", control_mode_params)
         while not result.success:
             result = vu.send_new_control_mode("left_arm", control_mode_params)
         print("done")
     else:
-        print("not using left arm")    
+        print("not using left arm")
 
-    if(use_right_arm):
+    if use_right_arm:
         print("initializing right arm ...")
-        r_cm = rh.Listener("right_arm/control_mode_status", ControlModeParameters)
+        r_cm = Listener("right_arm/control_mode_status", ControlModeParameters)
         cur_mode = r_cm.get(block_until_data=True)
         control_mode_params.joint_path_execution_params.joint_relative_velocity = cur_mode.joint_path_execution_params.joint_relative_velocity
         control_mode_params.joint_path_execution_params.joint_relative_acceleration = cur_mode.joint_path_execution_params.joint_relative_acceleration
@@ -143,18 +139,15 @@ def print_joints(left, right):
             result = vu.send_new_control_mode("right_arm", control_mode_params)
         print("done")
     else:
-        print("not using right arm")    
-
+        print("not using right arm")
 
     left = None
     right = None
-    if(use_left_arm):
+    if use_left_arm:
         left = ManualMotion("left_arm")
-    if(use_right_arm):
+    if use_right_arm:
         right = ManualMotion("right_arm")
 
-
-
     rospy.on_shutdown(lambda: print_joints(left, right))
 
     print("Running")