├── .gitignore ├── LICENSE ├── README.md ├── control_library ├── control_library │ ├── __init__.py │ ├── bezier.py │ ├── body_ik.py │ ├── gait_parameters.py │ ├── gait_planner.py │ └── leg_ik.py └── setup.py ├── depracated └── RPi │ └── main_interface.py ├── hardware ├── 3d-printing │ ├── Body │ │ ├── step │ │ │ ├── Adapter_Plate.step │ │ │ ├── Back_Inner_Shoulder.step │ │ │ ├── Back_Outer_Shoulder.step │ │ │ ├── Chassis_Left_Side - Chassis_Right_Side.step │ │ │ ├── Chassis_Left_Side.step │ │ │ ├── Electronics_Plate.step │ │ │ ├── Front_Inner_Shoulder.step │ │ │ └── Front_Outer_Shoulder.step │ │ └── stl │ │ │ ├── Adapter_Plate.stl │ │ │ ├── Back_Inner_Shoulder.stl │ │ │ ├── Back_Outer_Shoulder.stl │ │ │ ├── Chassis_Left_Side.stl │ │ │ ├── Chassis_Right_Side.stl │ │ │ ├── Electronics_Plate.stl │ │ │ ├── Front_Inner_Shoulder.stl │ │ │ └── Front_Outer_Shoulder.stl │ ├── Calibration │ │ ├── Hip_Calibrator.stl │ │ ├── L_Upper_Leg_Calibrator.stl │ │ └── Lower_Leg_Calibrator.stl │ ├── Covers │ │ ├── step │ │ │ ├── Bottom.step │ │ │ ├── Front.step │ │ │ ├── Rear.step │ │ │ └── Top.step │ │ └── stl │ │ │ ├── Bottom.stl │ │ │ ├── Front.stl │ │ │ ├── Rear.stl │ │ │ └── Top.stl │ ├── Legs │ │ ├── Left │ │ │ ├── step │ │ │ │ ├── Bridge.step │ │ │ │ ├── Foot.step │ │ │ │ ├── Idler_Mount.step │ │ │ │ ├── Left_Hip_Joint.step │ │ │ │ ├── Lower_Leg.step │ │ │ │ ├── Servo_Cover.step │ │ │ │ ├── Upper_Leg.step │ │ │ │ └── Upper_Pulley.step │ │ │ └── stl │ │ │ │ ├── Bridge.stl │ │ │ │ ├── Foot.stl │ │ │ │ ├── Idler_Mount.stl │ │ │ │ ├── Left_Hip_Joint.stl │ │ │ │ ├── Lower_Leg.stl │ │ │ │ ├── Servo_Cover.stl │ │ │ │ ├── Upper_Leg.stl │ │ │ │ └── Upper_Pulley.stl │ │ └── Right │ │ │ ├── step │ │ │ ├── Bridge.step │ │ │ ├── Foot.step │ │ │ ├── Idler_Mount.step │ │ │ ├── Lower_Leg.step │ │ │ ├── Right_Hip_Joint.step │ │ │ ├── Servo_Cover.step │ │ │ ├── Upper_Leg.step │ │ │ └── Upper_Pulley.step │ │ │ └── stl │ │ │ ├── Bridge.stl │ │ │ ├── Foot.stl │ │ │ ├── Idler_Mount.stl │ │ │ ├── Lower_Leg.stl │ │ │ ├── Right_Hip_Joint.stl │ │ │ ├── Servo_Cover.stl │ │ │ ├── Upper_Leg.stl │ │ │ └── Upper_Pulley.stl │ ├── README.md │ ├── Thumbs.db │ └── sls │ │ ├── OpenQuad_0.form │ │ └── OpenQuad_1.form └── pcb │ ├── DoublePCB.png │ ├── Gerber_DoublePCB.zip │ ├── Gerber_MainPCB.zip │ └── SinglePCB.png ├── llc-teensy ├── .gitignore ├── .travis.yml ├── include │ └── README ├── lib │ ├── AdvServo │ │ ├── AdvServo.cpp │ │ └── AdvServo.h │ ├── FootSensor │ │ ├── FootSensor.cpp │ │ └── FootSensor.h │ ├── InverseKinematics │ │ ├── InverseKinematics.cpp │ │ └── InverseKinematics.h │ ├── README │ ├── ROSSerial │ │ └── ROSSerial.hpp │ ├── Util │ │ ├── Util.cpp │ │ └── Util.h │ └── ros_lib │ │ ├── ArduinoHardware.h │ │ ├── actionlib │ │ ├── TestAction.h │ │ ├── TestActionFeedback.h │ │ ├── TestActionGoal.h │ │ ├── TestActionResult.h │ │ ├── TestFeedback.h │ │ ├── TestGoal.h │ │ ├── TestRequestAction.h │ │ ├── TestRequestActionFeedback.h │ │ ├── TestRequestActionGoal.h │ │ ├── TestRequestActionResult.h │ │ ├── TestRequestFeedback.h │ │ ├── TestRequestGoal.h │ │ ├── TestRequestResult.h │ │ ├── TestResult.h │ │ ├── TwoIntsAction.h │ │ ├── TwoIntsActionFeedback.h │ │ ├── TwoIntsActionGoal.h │ │ ├── TwoIntsActionResult.h │ │ ├── TwoIntsFeedback.h │ │ ├── TwoIntsGoal.h │ │ └── TwoIntsResult.h │ │ ├── actionlib_msgs │ │ ├── GoalID.h │ │ ├── GoalStatus.h │ │ └── GoalStatusArray.h │ │ ├── actionlib_tutorials │ │ ├── AveragingAction.h │ │ ├── AveragingActionFeedback.h │ │ ├── AveragingActionGoal.h │ │ ├── AveragingActionResult.h │ │ ├── AveragingFeedback.h │ │ ├── AveragingGoal.h │ │ ├── AveragingResult.h │ │ ├── FibonacciAction.h │ │ ├── FibonacciActionFeedback.h │ │ ├── FibonacciActionGoal.h │ │ ├── FibonacciActionResult.h │ │ ├── FibonacciFeedback.h │ │ ├── FibonacciGoal.h │ │ └── FibonacciResult.h │ │ ├── bond │ │ ├── Constants.h │ │ └── Status.h │ │ ├── control_msgs │ │ ├── FollowJointTrajectoryAction.h │ │ ├── FollowJointTrajectoryActionFeedback.h │ │ ├── FollowJointTrajectoryActionGoal.h │ │ ├── FollowJointTrajectoryActionResult.h │ │ ├── FollowJointTrajectoryFeedback.h │ │ ├── FollowJointTrajectoryGoal.h │ │ ├── FollowJointTrajectoryResult.h │ │ ├── GripperCommand.h │ │ ├── GripperCommandAction.h │ │ ├── GripperCommandActionFeedback.h │ │ ├── GripperCommandActionGoal.h │ │ ├── GripperCommandActionResult.h │ │ ├── GripperCommandFeedback.h │ │ ├── GripperCommandGoal.h │ │ ├── GripperCommandResult.h │ │ ├── JointControllerState.h │ │ ├── JointJog.h │ │ ├── JointTolerance.h │ │ ├── JointTrajectoryAction.h │ │ ├── JointTrajectoryActionFeedback.h │ │ ├── JointTrajectoryActionGoal.h │ │ ├── JointTrajectoryActionResult.h │ │ ├── JointTrajectoryControllerState.h │ │ ├── JointTrajectoryFeedback.h │ │ ├── JointTrajectoryGoal.h │ │ ├── JointTrajectoryResult.h │ │ ├── PidState.h │ │ ├── PointHeadAction.h │ │ ├── PointHeadActionFeedback.h │ │ ├── PointHeadActionGoal.h │ │ ├── PointHeadActionResult.h │ │ ├── PointHeadFeedback.h │ │ ├── PointHeadGoal.h │ │ ├── PointHeadResult.h │ │ ├── QueryCalibrationState.h │ │ ├── QueryTrajectoryState.h │ │ ├── SingleJointPositionAction.h │ │ ├── SingleJointPositionActionFeedback.h │ │ ├── SingleJointPositionActionGoal.h │ │ ├── SingleJointPositionActionResult.h │ │ ├── SingleJointPositionFeedback.h │ │ ├── SingleJointPositionGoal.h │ │ └── SingleJointPositionResult.h │ │ ├── diagnostic_msgs │ │ ├── AddDiagnostics.h │ │ ├── DiagnosticArray.h │ │ ├── DiagnosticStatus.h │ │ ├── KeyValue.h │ │ └── SelfTest.h │ │ ├── duration.cpp │ │ ├── dynamic_reconfigure │ │ ├── BoolParameter.h │ │ ├── Config.h │ │ ├── ConfigDescription.h │ │ ├── DoubleParameter.h │ │ ├── Group.h │ │ ├── GroupState.h │ │ ├── IntParameter.h │ │ ├── ParamDescription.h │ │ ├── Reconfigure.h │ │ ├── SensorLevels.h │ │ └── StrParameter.h │ │ ├── geometry_msgs │ │ ├── Accel.h │ │ ├── AccelStamped.h │ │ ├── AccelWithCovariance.h │ │ ├── AccelWithCovarianceStamped.h │ │ ├── Inertia.h │ │ ├── InertiaStamped.h │ │ ├── Point.h │ │ ├── Point32.h │ │ ├── PointStamped.h │ │ ├── Polygon.h │ │ ├── PolygonStamped.h │ │ ├── Pose.h │ │ ├── Pose2D.h │ │ ├── PoseArray.h │ │ ├── PoseStamped.h │ │ ├── PoseWithCovariance.h │ │ ├── PoseWithCovarianceStamped.h │ │ ├── Quaternion.h │ │ ├── QuaternionStamped.h │ │ ├── Transform.h │ │ ├── TransformStamped.h │ │ ├── Twist.h │ │ ├── TwistStamped.h │ │ ├── TwistWithCovariance.h │ │ ├── TwistWithCovarianceStamped.h │ │ ├── Vector3.h │ │ ├── Vector3Stamped.h │ │ ├── Wrench.h │ │ └── WrenchStamped.h │ │ ├── map_msgs │ │ ├── GetMapROI.h │ │ ├── GetPointMap.h │ │ ├── GetPointMapROI.h │ │ ├── OccupancyGridUpdate.h │ │ ├── PointCloud2Update.h │ │ ├── ProjectedMap.h │ │ ├── ProjectedMapInfo.h │ │ ├── ProjectedMapsInfo.h │ │ ├── SaveMap.h │ │ └── SetMapProjections.h │ │ ├── nav_msgs │ │ ├── GetMap.h │ │ ├── GetMapAction.h │ │ ├── GetMapActionFeedback.h │ │ ├── GetMapActionGoal.h │ │ ├── GetMapActionResult.h │ │ ├── GetMapFeedback.h │ │ ├── GetMapGoal.h │ │ ├── GetMapResult.h │ │ ├── GetPlan.h │ │ ├── GridCells.h │ │ ├── MapMetaData.h │ │ ├── OccupancyGrid.h │ │ ├── Odometry.h │ │ ├── Path.h │ │ └── SetMap.h │ │ ├── nodelet │ │ ├── NodeletList.h │ │ ├── NodeletLoad.h │ │ └── NodeletUnload.h │ │ ├── open_quadruped │ │ └── JointAngles.h │ │ ├── ros.h │ │ ├── ros │ │ ├── duration.h │ │ ├── msg.h │ │ ├── node_handle.h │ │ ├── publisher.h │ │ ├── service_client.h │ │ ├── service_server.h │ │ ├── subscriber.h │ │ └── time.h │ │ ├── roscpp │ │ ├── Empty.h │ │ ├── GetLoggers.h │ │ ├── Logger.h │ │ └── SetLoggerLevel.h │ │ ├── roscpp_tutorials │ │ └── TwoInts.h │ │ ├── rosgraph_msgs │ │ ├── Clock.h │ │ ├── Log.h │ │ └── TopicStatistics.h │ │ ├── rospy_tutorials │ │ ├── AddTwoInts.h │ │ ├── BadTwoInts.h │ │ ├── Floats.h │ │ └── HeaderString.h │ │ ├── rosserial_arduino │ │ ├── Adc.h │ │ └── Test.h │ │ ├── rosserial_msgs │ │ ├── Log.h │ │ ├── RequestMessageInfo.h │ │ ├── RequestParam.h │ │ ├── RequestServiceInfo.h │ │ └── TopicInfo.h │ │ ├── sensor_msgs │ │ ├── BatteryState.h │ │ ├── CameraInfo.h │ │ ├── ChannelFloat32.h │ │ ├── CompressedImage.h │ │ ├── FluidPressure.h │ │ ├── Illuminance.h │ │ ├── Image.h │ │ ├── Imu.h │ │ ├── JointState.h │ │ ├── Joy.h │ │ ├── JoyFeedback.h │ │ ├── JoyFeedbackArray.h │ │ ├── LaserEcho.h │ │ ├── LaserScan.h │ │ ├── MagneticField.h │ │ ├── MultiDOFJointState.h │ │ ├── MultiEchoLaserScan.h │ │ ├── NavSatFix.h │ │ ├── NavSatStatus.h │ │ ├── PointCloud.h │ │ ├── PointCloud2.h │ │ ├── PointField.h │ │ ├── Range.h │ │ ├── RegionOfInterest.h │ │ ├── RelativeHumidity.h │ │ ├── SetCameraInfo.h │ │ ├── Temperature.h │ │ └── TimeReference.h │ │ ├── shape_msgs │ │ ├── Mesh.h │ │ ├── MeshTriangle.h │ │ ├── Plane.h │ │ └── SolidPrimitive.h │ │ ├── smach_msgs │ │ ├── SmachContainerInitialStatusCmd.h │ │ ├── SmachContainerStatus.h │ │ └── SmachContainerStructure.h │ │ ├── std_msgs │ │ ├── Bool.h │ │ ├── Byte.h │ │ ├── ByteMultiArray.h │ │ ├── Char.h │ │ ├── ColorRGBA.h │ │ ├── Duration.h │ │ ├── Empty.h │ │ ├── Float32.h │ │ ├── Float32MultiArray.h │ │ ├── Float64.h │ │ ├── Float64MultiArray.h │ │ ├── Header.h │ │ ├── Int16.h │ │ ├── Int16MultiArray.h │ │ ├── Int32.h │ │ ├── Int32MultiArray.h │ │ ├── Int64.h │ │ ├── Int64MultiArray.h │ │ ├── Int8.h │ │ ├── Int8MultiArray.h │ │ ├── MultiArrayDimension.h │ │ ├── MultiArrayLayout.h │ │ ├── String.h │ │ ├── Time.h │ │ ├── UInt16.h │ │ ├── UInt16MultiArray.h │ │ ├── UInt32.h │ │ ├── UInt32MultiArray.h │ │ ├── UInt64.h │ │ ├── UInt64MultiArray.h │ │ ├── UInt8.h │ │ └── UInt8MultiArray.h │ │ ├── std_srvs │ │ ├── Empty.h │ │ ├── SetBool.h │ │ └── Trigger.h │ │ ├── stereo_msgs │ │ └── DisparityImage.h │ │ ├── tf │ │ ├── FrameGraph.h │ │ ├── tf.h │ │ ├── tfMessage.h │ │ └── transform_broadcaster.h │ │ ├── tf2_msgs │ │ ├── FrameGraph.h │ │ ├── LookupTransformAction.h │ │ ├── LookupTransformActionFeedback.h │ │ ├── LookupTransformActionGoal.h │ │ ├── LookupTransformActionResult.h │ │ ├── LookupTransformFeedback.h │ │ ├── LookupTransformGoal.h │ │ ├── LookupTransformResult.h │ │ ├── TF2Error.h │ │ └── TFMessage.h │ │ ├── time.cpp │ │ ├── topic_tools │ │ ├── DemuxAdd.h │ │ ├── DemuxDelete.h │ │ ├── DemuxList.h │ │ ├── DemuxSelect.h │ │ ├── MuxAdd.h │ │ ├── MuxDelete.h │ │ ├── MuxList.h │ │ └── MuxSelect.h │ │ ├── trajectory_msgs │ │ ├── JointTrajectory.h │ │ ├── JointTrajectoryPoint.h │ │ ├── MultiDOFJointTrajectory.h │ │ └── MultiDOFJointTrajectoryPoint.h │ │ ├── turtle_actionlib │ │ ├── ShapeAction.h │ │ ├── ShapeActionFeedback.h │ │ ├── ShapeActionGoal.h │ │ ├── ShapeActionResult.h │ │ ├── ShapeFeedback.h │ │ ├── ShapeGoal.h │ │ ├── ShapeResult.h │ │ └── Velocity.h │ │ ├── turtlesim │ │ ├── Color.h │ │ ├── Kill.h │ │ ├── Pose.h │ │ ├── SetPen.h │ │ ├── Spawn.h │ │ ├── TeleportAbsolute.h │ │ └── TeleportRelative.h │ │ └── visualization_msgs │ │ ├── ImageMarker.h │ │ ├── InteractiveMarker.h │ │ ├── InteractiveMarkerControl.h │ │ ├── InteractiveMarkerFeedback.h │ │ ├── InteractiveMarkerInit.h │ │ ├── InteractiveMarkerPose.h │ │ ├── InteractiveMarkerUpdate.h │ │ ├── Marker.h │ │ ├── MarkerArray.h │ │ └── MenuEntry.h ├── platformio.ini ├── src │ └── main.cpp └── test │ └── README ├── media ├── OpenQuadruped Node Infrastructure.png ├── OpenQuadruped.png ├── SideView.png ├── bodyik_demo.gif ├── trot_demo.gif └── wiring_diagram.png ├── ros-workspace ├── .catkin_workspace └── src │ ├── CMakeLists.txt │ └── open_quadruped │ ├── CMakeLists.txt │ ├── \ │ ├── config │ └── control.yaml │ ├── launch │ ├── real.launch │ └── sim.launch │ ├── msg │ └── JointAngles.msg │ ├── nodes │ ├── \ │ └── interface_process.py │ ├── package.xml │ └── urdf │ ├── accessories.urdf.xacro │ ├── black │ ├── spot.urdf │ ├── spot.urdf.xacro │ ├── spot_accessories.urdf │ ├── spot_macro.xacro │ ├── stl │ ├── LICENSE.txt │ ├── OpenQuadruped │ │ ├── Back.stl │ │ ├── Back_Bracket.stl │ │ ├── Battery.stl │ │ ├── Chassis_Left_Side.stl │ │ ├── Chassis_Right_Side.stl │ │ ├── Front.stl │ │ ├── Front_Bracket.stl │ │ ├── LEFT_FOOT.stl │ │ ├── LEFT_HIP.stl │ │ ├── LEFT_LOWER_LEG.stl │ │ ├── LEFT_UPPER_LEG.stl │ │ ├── MAINBODY.stl │ │ ├── RIGHT_FOOT.stl │ │ ├── RIGHT_HIP.stl │ │ ├── RIGHT_LOWER_LEG.stl │ │ ├── RIGHT_UPPER_LEG.stl │ │ └── SERVO_WITH_COVER.stl │ ├── README.txt │ ├── backpart.stl │ ├── foot.stl │ ├── frontpart.stl │ ├── larm.stl │ ├── larm_cover.stl │ ├── lfoot.stl │ ├── lshoulder.stl │ ├── mainbody.dae │ ├── mainbody.stl │ ├── rarm.stl │ ├── rarm_cover.stl │ ├── rear.stl │ ├── rear2.stl │ ├── rearmain.stl │ ├── rfoot.stl │ ├── rplidar_main.stl │ └── rshoulder.stl │ └── transmissions.xacro └── vis-tool ├── IK_Engine.py ├── README.md ├── animate.py ├── interface.py └── test.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Adham Elarabawy 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | In order to use any of the works or derivative works that this license covers, 24 | explicit & formal credit must be granted to the original creator (Adham Elarabawy). 25 | Any violation of this clause will result in a DMCA claim against the violator. 26 | -------------------------------------------------------------------------------- /control_library/control_library/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/control_library/control_library/__init__.py -------------------------------------------------------------------------------- /control_library/control_library/gait_parameters.py: -------------------------------------------------------------------------------- 1 | class GaitParameters: 2 | def __init__(self, phase_lag, T_swing, L_span, v_d, penetration_alpha, base_height, y, x_shift, clearance): 3 | self.phase_lag = phase_lag 4 | self.T_swing = T_swing 5 | self.T_stance = 2 * L_span / v_d 6 | self.L_span = L_span 7 | self.v_d = v_d 8 | self.penetration_alpha = penetration_alpha 9 | self.base_height = base_height 10 | self.y = y 11 | self.x_shift = x_shift 12 | self.clearance = clearance 13 | -------------------------------------------------------------------------------- /control_library/control_library/gait_planner.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class GaitPlanner: 5 | def __init__(self, T_stance, T_swing, phase_lag): 6 | self.T_stance = T_stance 7 | self.T_swing = T_swing 8 | self.phase_lag = phase_lag 9 | self.T_stride = self.T_swing + self.T_stance 10 | 11 | def signal_sample(self, time, leg): 12 | phase_i = self.phase_lag[leg] 13 | phase_time = phase_i * self.T_stride 14 | 15 | base_time = (time - phase_time) % self.T_stride 16 | while base_time < 0: 17 | base_time = self.T_stride - base_time 18 | 19 | if base_time <= self.T_stance: 20 | # in stride period 21 | return [0, base_time / self.T_stance] 22 | else: 23 | # in swing period 24 | return [1, (base_time - self.T_stance) / self.T_swing] 25 | -------------------------------------------------------------------------------- /control_library/control_library/leg_ik.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | class LegIKModel: 4 | def __init__(self, upper, lower, off0, off1): 5 | """Leg Inverse Kinematics Model. Based on Adham Elarabawy's paper: www.adham-e.dev/papers 6 | 7 | args: upper, lower, off0, off1 8 | degs: length of the upper arm (pivot to pivot in mm), length of the lower arm (pivot to pivot in mm), off0 from paper, off1 from paper 9 | """ 10 | self.upper = upper 11 | self.lower = lower 12 | self.off0 = off0 13 | self.off1 = off1 14 | 15 | def ja_from_htf_vecs(self, htf_vecs): 16 | joint_angles = [] 17 | try: 18 | for i, (x, y, z) in enumerate(htf_vecs): 19 | h1 = math.sqrt(self.off0**2 + self.off1**2) 20 | h2 = math.sqrt(z**2 + y**2) 21 | alpha_0 = math.atan(y / z) 22 | alpha_1 = math.atan(self.off1 / self.off0) 23 | alpha_2 = math.atan(self.off0 / self.off1) 24 | alpha_3 = math.asin( 25 | h1 * math.sin(alpha_2 + math.radians(90)) / h2) 26 | alpha_4 = math.radians( 27 | 180) - (alpha_3 + alpha_2 + math.radians(90)) 28 | alpha_5 = alpha_1 - alpha_4 29 | theta_h = alpha_0 - alpha_5 30 | 31 | r0 = h1 * math.sin(alpha_4) / math.sin(alpha_3) 32 | h = math.sqrt(r0**2 + x**2) 33 | phi = math.asin(x / h) 34 | theta_s = math.acos( 35 | (h**2 + self.upper**2 - self.lower**2) / (2 * h * self.upper)) - phi 36 | theta_w = math.acos((self.lower**2 + self.upper ** 37 | 2 - h**2) / (2 * self.lower * self.upper)) 38 | joint_angles.append([theta_h, theta_s, theta_w]) 39 | except: 40 | print("Out of Bounds.") 41 | 42 | return joint_angles 43 | -------------------------------------------------------------------------------- /control_library/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup(name='control_library', 4 | version='0.1', 5 | description='A control library for the OpenQuadruped project', 6 | url='https://github.com/adham-elarabawy/open-quadruped', 7 | author='Adham Elarabawy', 8 | author_email='aelarabawy@berkeley.edu', 9 | license='MIT', 10 | packages=['control_library'], 11 | zip_safe=False) 12 | -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Adapter_Plate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Adapter_Plate.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Back_Inner_Shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Back_Inner_Shoulder.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Back_Outer_Shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Back_Outer_Shoulder.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Chassis_Left_Side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Chassis_Left_Side.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Chassis_Right_Side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Chassis_Right_Side.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Electronics_Plate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Electronics_Plate.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Front_Inner_Shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Front_Inner_Shoulder.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Body/stl/Front_Outer_Shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Body/stl/Front_Outer_Shoulder.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Calibration/Hip_Calibrator.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Calibration/Hip_Calibrator.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Calibration/L_Upper_Leg_Calibrator.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Calibration/L_Upper_Leg_Calibrator.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Calibration/Lower_Leg_Calibrator.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Calibration/Lower_Leg_Calibrator.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Covers/stl/Bottom.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Covers/stl/Bottom.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Covers/stl/Front.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Covers/stl/Front.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Covers/stl/Rear.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Covers/stl/Rear.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Covers/stl/Top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Covers/stl/Top.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Bridge.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Bridge.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Foot.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Idler_Mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Idler_Mount.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Left_Hip_Joint.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Left_Hip_Joint.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Lower_Leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Lower_Leg.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Servo_Cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Servo_Cover.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Upper_Leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Upper_Leg.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Left/stl/Upper_Pulley.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Left/stl/Upper_Pulley.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Bridge.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Bridge.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Foot.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Idler_Mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Idler_Mount.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Lower_Leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Lower_Leg.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Right_Hip_Joint.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Right_Hip_Joint.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Servo_Cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Servo_Cover.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Upper_Leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Upper_Leg.stl -------------------------------------------------------------------------------- /hardware/3d-printing/Legs/Right/stl/Upper_Pulley.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Legs/Right/stl/Upper_Pulley.stl -------------------------------------------------------------------------------- /hardware/3d-printing/README.md: -------------------------------------------------------------------------------- 1 | # 3D Model 2 | In this folder, you can find all of the modified step & stl files that I used for my build of OpenQuadruped. 3 | 4 | ## Instructions 5 | In Progress... 6 | -------------------------------------------------------------------------------- /hardware/3d-printing/Thumbs.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/Thumbs.db -------------------------------------------------------------------------------- /hardware/3d-printing/sls/OpenQuad_0.form: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/sls/OpenQuad_0.form -------------------------------------------------------------------------------- /hardware/3d-printing/sls/OpenQuad_1.form: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/3d-printing/sls/OpenQuad_1.form -------------------------------------------------------------------------------- /hardware/pcb/DoublePCB.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/pcb/DoublePCB.png -------------------------------------------------------------------------------- /hardware/pcb/Gerber_DoublePCB.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/pcb/Gerber_DoublePCB.zip -------------------------------------------------------------------------------- /hardware/pcb/Gerber_MainPCB.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/pcb/Gerber_MainPCB.zip -------------------------------------------------------------------------------- /hardware/pcb/SinglePCB.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/hardware/pcb/SinglePCB.png -------------------------------------------------------------------------------- /llc-teensy/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .clang_complete 3 | .gcc-flags.json 4 | -------------------------------------------------------------------------------- /llc-teensy/.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < https://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < https://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < https://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choose one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | # language: python 28 | # python: 29 | # - "2.7" 30 | # 31 | # sudo: false 32 | # cache: 33 | # directories: 34 | # - "~/.platformio" 35 | # 36 | # install: 37 | # - pip install -U platformio 38 | # - platformio update 39 | # 40 | # script: 41 | # - platformio run 42 | 43 | 44 | # 45 | # Template #2: The project is intended to be used as a library with examples. 46 | # 47 | 48 | # language: python 49 | # python: 50 | # - "2.7" 51 | # 52 | # sudo: false 53 | # cache: 54 | # directories: 55 | # - "~/.platformio" 56 | # 57 | # env: 58 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 59 | # - PLATFORMIO_CI_SRC=examples/file.ino 60 | # - PLATFORMIO_CI_SRC=path/to/test/directory 61 | # 62 | # install: 63 | # - pip install -U platformio 64 | # - platformio update 65 | # 66 | # script: 67 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 68 | -------------------------------------------------------------------------------- /llc-teensy/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /llc-teensy/lib/AdvServo/AdvServo.cpp: -------------------------------------------------------------------------------- 1 | #include "AdvServo.h" 2 | #include 3 | #include 4 | 5 | void AdvServo::init(int servo_pin, double starting_angle, double tempOffset) { 6 | servo.attach(servo_pin, 500, 2500); 7 | off = tempOffset; 8 | pos = starting_angle + off; 9 | servo.write((starting_angle + off) * 180 / control_range); 10 | delay(1000); 11 | last_actuated = millis(); 12 | } 13 | 14 | void AdvServo::setType(int tempLegNum, int tempJointNum) { 15 | leg = tempLegNum; 16 | joint = tempJointNum; 17 | } 18 | 19 | void AdvServo::setPosition(double tempPos, double tempSpeed) { 20 | tempPos = tempPos + off; 21 | if(tempPos > control_range) { 22 | // Serial.println("Attempted to set position out of control range. Capped at max."); 23 | tempPos = control_range; 24 | } 25 | 26 | if(tempPos < 0) { 27 | // Serial.println("Attempted to set position out of control range. Capped at min."); 28 | tempPos = 0; 29 | } 30 | 31 | goal = tempPos; 32 | spd = tempSpeed; 33 | } 34 | 35 | int AdvServo::getPosition() { 36 | return pos - off; 37 | } 38 | 39 | void AdvServo::update_clk() { 40 | if(millis() - last_actuated > wait_time){ 41 | if(abs(pos - goal) > error_threshold) { 42 | int dir = 0; 43 | if(goal - pos > 0) { 44 | dir = 1; 45 | } else if(goal - pos < 0) { 46 | dir = -1; 47 | } 48 | pos += dir * wait_time / 1000 * spd; 49 | servo.write(pos * 180 / control_range); 50 | last_actuated = millis(); 51 | } 52 | } 53 | } 54 | 55 | void AdvServo::detach() { 56 | servo.detach(); 57 | } 58 | -------------------------------------------------------------------------------- /llc-teensy/lib/AdvServo/AdvServo.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class AdvServo { 4 | private: 5 | Servo servo; 6 | double last_actuated; 7 | double error_threshold = 0.5; 8 | int control_range = 270; 9 | double wait_time = 1; 10 | 11 | public: 12 | double goal; 13 | double pos; 14 | double spd; 15 | int off = 0; 16 | int leg; // 0: Front Left, 1: Front Right, 2: Back Left, 3: Back Right 17 | int joint; // 0: Hip, 1: Shoulder, 2: Wrist 18 | void init(int servo_pin, double starting_angle, double off); 19 | void setType(int tempLegNum, int tempJointNum); 20 | void setPosition(double tempPos, double tempSpeed); 21 | int getPosition(); 22 | void update_clk(); 23 | void detach(); 24 | }; 25 | -------------------------------------------------------------------------------- /llc-teensy/lib/FootSensor/FootSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "FootSensor.h" 2 | #include 3 | 4 | void FootSensor::init(int in_sensor_pin, int in_led_pin) { 5 | sensor_pin = in_sensor_pin; 6 | led_pin = in_led_pin; 7 | 8 | pinMode(led_pin, OUTPUT); 9 | 10 | double av = 0; 11 | for(int i=0; i < numSamples; i++) { 12 | av += analogRead(sensor_pin); 13 | delay(5); 14 | } 15 | center = av / numSamples; 16 | } 17 | 18 | void FootSensor::update_clk() { 19 | val = alpha * prev_val + (1 - alpha) * abs(analogRead(sensor_pin) - center); // read the input pin 20 | if(val > thresh) { 21 | digitalWrite(led_pin, HIGH); 22 | } else { 23 | digitalWrite(led_pin, LOW); 24 | } 25 | prev_val = val; 26 | } 27 | 28 | bool FootSensor::isTriggered() { 29 | if(val > thresh) { 30 | return true; 31 | } 32 | return false; 33 | } 34 | -------------------------------------------------------------------------------- /llc-teensy/lib/FootSensor/FootSensor.h: -------------------------------------------------------------------------------- 1 | class FootSensor { 2 | private: 3 | int led_pin; 4 | int sensor_pin; 5 | double center = 0; 6 | double prev_val = 0; 7 | double val = 0; 8 | int numSamples = 200; 9 | double thresh = 2.5; 10 | double alpha = 0.999; 11 | 12 | public: 13 | void init(int in_sensor_pin, int in_led_pin); 14 | bool isTriggered(); 15 | void update_clk(); 16 | }; 17 | -------------------------------------------------------------------------------- /llc-teensy/lib/InverseKinematics/InverseKinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "InverseKinematics.h" 2 | #include 3 | #include 4 | 5 | void InverseKinematics::init(double hip_offset_0, double hip_offset_1, double temp_shoulder, double temp_wrist) { 6 | hip_offset[0] = hip_offset_0; 7 | hip_offset[1] = hip_offset_1; 8 | shoulder = temp_shoulder; 9 | wrist = temp_wrist; 10 | } 11 | 12 | double * InverseKinematics::run(int legIndex, double x, double y, double z) { 13 | static double angles[3]; 14 | 15 | double h1 = sqrt(pow(hip_offset[0], 2) + pow(hip_offset[1], 2)); 16 | double h2 = sqrt(pow(z, 2) + pow(y, 2)); 17 | double alpha_0 = atan(y / z); 18 | double alpha_1 = atan(hip_offset[1] / hip_offset[0]); 19 | double alpha_2 = atan(hip_offset[0] / hip_offset[1]); 20 | double alpha_3 = asin(h1 * sin(alpha_2 + HALF_PI) / h2); 21 | double alpha_4 = PI - (alpha_3 + alpha_2 + HALF_PI); 22 | double alpha_5 = alpha_1 - alpha_4; 23 | double theta_h = alpha_0 - alpha_5; 24 | 25 | double r0 = h1 * sin(alpha_4) / sin(alpha_3); 26 | double h = sqrt(pow(r0, 2) + pow(x, 2)); 27 | double phi = asin(x / h); 28 | double theta_s = acos((pow(h, 2) + pow(shoulder, 2) - pow(wrist, 2)) / (2 * h * shoulder)) - phi; 29 | double theta_w = acos((pow(wrist, 2) + pow(shoulder, 2) - pow(h, 2)) / (2 * wrist * shoulder)); 30 | 31 | angles[0] = theta_h; 32 | angles[1] = theta_s; 33 | angles[2] = theta_w; 34 | 35 | if(legIndex < 2) { 36 | return angles; 37 | } else { 38 | angles[0] = -theta_h; 39 | return angles; 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /llc-teensy/lib/InverseKinematics/InverseKinematics.h: -------------------------------------------------------------------------------- 1 | class InverseKinematics { 2 | private: 3 | double hip_offset[2]; 4 | double wrist; 5 | double shoulder; 6 | 7 | public: 8 | void init(double hip_offset_0, double hip_offset_1, double temp_shoulder, double temp_wrist); 9 | double * run(int legIndex, double x, double y, double z); 10 | }; 11 | -------------------------------------------------------------------------------- /llc-teensy/lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /llc-teensy/lib/Util/Util.cpp: -------------------------------------------------------------------------------- 1 | #include "Util.h" 2 | #include 3 | 4 | void Util::upper(char* s) { 5 | for(int i = 0; i < strlen(s); i++){ 6 | s[i] = toupper(s[i]); 7 | } 8 | } 9 | 10 | double Util::max(double a0, double a1, double a2) { 11 | if(a0 >= a1 && a0 >= a2) { 12 | return a0; 13 | } 14 | if(a1 >= a0 && a1 >= a2) { 15 | return a1; 16 | } 17 | if(a2 >= a1 && a2 >= a0) { 18 | return a2; 19 | } 20 | } 21 | 22 | double Util::toDegrees(double radianVal) { 23 | return radianVal * 57296 / 1000; 24 | } 25 | 26 | double Util::angleConversion(int leg, int joint, double angle) { 27 | if(joint == 0){ 28 | if(leg == 0 || leg == 1) { 29 | angle = -angle; 30 | } 31 | angle = angle + 135; 32 | } 33 | 34 | if(joint == 1) { 35 | if(leg == 0 || leg == 2) { 36 | angle = 90 + angle; 37 | } 38 | if(leg == 1 || leg == 3) { 39 | angle = 180 - angle; 40 | } 41 | } 42 | 43 | if(joint == 2) { 44 | double weird_offset = 50; 45 | if(leg == 0 || leg == 2) { 46 | angle = angle - weird_offset; 47 | } 48 | if(leg == 1 || leg == 3) { 49 | angle = (270 + weird_offset) - angle; 50 | } 51 | } 52 | return angle; 53 | } 54 | 55 | int Util::inverse_angleConversion(int leg, int joint, double angle) { 56 | if(joint == 0){ 57 | if (leg == 0 || leg == 1) { 58 | angle = 135 - angle; 59 | } 60 | if (leg == 2 || leg == 3) { 61 | angle = angle - 135; 62 | } 63 | } 64 | 65 | if(joint == 1) { 66 | if(leg == 0 || leg == 2) { 67 | angle = angle - 90; 68 | } 69 | if(leg == 1 || leg == 3) { 70 | angle = 180 - angle; 71 | } 72 | } 73 | 74 | if(joint == 2) { 75 | double weird_offset = 50; 76 | if(leg == 0 || leg == 2) { 77 | angle = weird_offset + angle; 78 | } 79 | if(leg == 1 || leg == 3) { 80 | angle = (270 + weird_offset) - angle; 81 | } 82 | } 83 | return angle; 84 | } 85 | -------------------------------------------------------------------------------- /llc-teensy/lib/Util/Util.h: -------------------------------------------------------------------------------- 1 | class Util { 2 | public: 3 | void Util::upper(char* s); 4 | double angleConversion(int leg, int joint, double angle); 5 | int inverse_angleConversion(int leg, int joint, double angle); 6 | double toDegrees(double radianVal); 7 | double max(double a0, double a1, double a2); 8 | }; 9 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestAction_h 2 | #define _ROS_actionlib_TestAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TestActionGoal.h" 9 | #include "actionlib/TestActionResult.h" 10 | #include "actionlib/TestActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestAction : public ros::Msg 16 | { 17 | public: 18 | typedef actionlib::TestActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef actionlib::TestActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef actionlib::TestActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | TestAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestAction"; }; 51 | const char * getMD5(){ return "991e87a72802262dfbe5d1b3cf6efc9a"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionFeedback_h 2 | #define _ROS_actionlib_TestActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TestFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | TestActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestActionFeedback"; }; 51 | const char * getMD5(){ return "6d3d0bf7fb3dda24779c010a9f3eb7cb"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionGoal_h 2 | #define _ROS_actionlib_TestActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TestGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef actionlib::TestGoal _goal_type; 23 | _goal_type goal; 24 | 25 | TestActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestActionGoal"; }; 51 | const char * getMD5(){ return "348369c5b403676156094e8c159720bf"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestActionResult_h 2 | #define _ROS_actionlib_TestActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TestResult _result_type; 23 | _result_type result; 24 | 25 | TestActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestActionResult"; }; 51 | const char * getMD5(){ return "3d669e3a63aa986c667ea7b0f46ce85e"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestFeedback_h 2 | #define _ROS_actionlib_TestFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestFeedback : public ros::Msg 13 | { 14 | public: 15 | typedef int32_t _feedback_type; 16 | _feedback_type feedback; 17 | 18 | TestFeedback(): 19 | feedback(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int32_t real; 28 | uint32_t base; 29 | } u_feedback; 30 | u_feedback.real = this->feedback; 31 | *(outbuffer + offset + 0) = (u_feedback.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_feedback.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_feedback.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_feedback.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->feedback); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | int32_t real; 44 | uint32_t base; 45 | } u_feedback; 46 | u_feedback.base = 0; 47 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_feedback.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->feedback = u_feedback.real; 52 | offset += sizeof(this->feedback); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "actionlib/TestFeedback"; }; 57 | const char * getMD5(){ return "49ceb5b32ea3af22073ede4a0328249e"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestGoal_h 2 | #define _ROS_actionlib_TestGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestGoal : public ros::Msg 13 | { 14 | public: 15 | typedef int32_t _goal_type; 16 | _goal_type goal; 17 | 18 | TestGoal(): 19 | goal(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int32_t real; 28 | uint32_t base; 29 | } u_goal; 30 | u_goal.real = this->goal; 31 | *(outbuffer + offset + 0) = (u_goal.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_goal.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_goal.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_goal.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->goal); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | int32_t real; 44 | uint32_t base; 45 | } u_goal; 46 | u_goal.base = 0; 47 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_goal.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->goal = u_goal.real; 52 | offset += sizeof(this->goal); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "actionlib/TestGoal"; }; 57 | const char * getMD5(){ return "18df0149936b7aa95588e3862476ebde"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestRequestAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestAction_h 2 | #define _ROS_actionlib_TestRequestAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TestRequestActionGoal.h" 9 | #include "actionlib/TestRequestActionResult.h" 10 | #include "actionlib/TestRequestActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestAction : public ros::Msg 16 | { 17 | public: 18 | typedef actionlib::TestRequestActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef actionlib::TestRequestActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef actionlib::TestRequestActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | TestRequestAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestRequestAction"; }; 51 | const char * getMD5(){ return "dc44b1f4045dbf0d1db54423b3b86b30"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestRequestActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionFeedback_h 2 | #define _ROS_actionlib_TestRequestActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestRequestFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TestRequestFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | TestRequestActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestRequestActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestRequestActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionGoal_h 2 | #define _ROS_actionlib_TestRequestActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TestRequestGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef actionlib::TestRequestGoal _goal_type; 23 | _goal_type goal; 24 | 25 | TestRequestActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestRequestActionGoal"; }; 51 | const char * getMD5(){ return "1889556d3fef88f821c7cb004e4251f3"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestRequestActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestActionResult_h 2 | #define _ROS_actionlib_TestRequestActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TestRequestResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TestRequestActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TestRequestResult _result_type; 23 | _result_type result; 24 | 25 | TestRequestActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TestRequestActionResult"; }; 51 | const char * getMD5(){ return "0476d1fdf437a3a6e7d6d0e9f5561298"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestRequestFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestRequestFeedback_h 2 | #define _ROS_actionlib_TestRequestFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestRequestFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | TestRequestFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "actionlib/TestRequestFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TestResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TestResult_h 2 | #define _ROS_actionlib_TestResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TestResult : public ros::Msg 13 | { 14 | public: 15 | typedef int32_t _result_type; 16 | _result_type result; 17 | 18 | TestResult(): 19 | result(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int32_t real; 28 | uint32_t base; 29 | } u_result; 30 | u_result.real = this->result; 31 | *(outbuffer + offset + 0) = (u_result.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_result.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_result.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_result.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->result); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | int32_t real; 44 | uint32_t base; 45 | } u_result; 46 | u_result.base = 0; 47 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_result.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->result = u_result.real; 52 | offset += sizeof(this->result); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "actionlib/TestResult"; }; 57 | const char * getMD5(){ return "034a8e20d6a306665e3a5b340fab3f09"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TwoIntsAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsAction_h 2 | #define _ROS_actionlib_TwoIntsAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "actionlib/TwoIntsActionGoal.h" 9 | #include "actionlib/TwoIntsActionResult.h" 10 | #include "actionlib/TwoIntsActionFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsAction : public ros::Msg 16 | { 17 | public: 18 | typedef actionlib::TwoIntsActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef actionlib::TwoIntsActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef actionlib::TwoIntsActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | TwoIntsAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TwoIntsAction"; }; 51 | const char * getMD5(){ return "6d1aa538c4bd6183a2dfb7fcac41ee50"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TwoIntsActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionFeedback_h 2 | #define _ROS_actionlib_TwoIntsActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TwoIntsFeedback.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TwoIntsFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | TwoIntsActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TwoIntsActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TwoIntsActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionGoal_h 2 | #define _ROS_actionlib_TwoIntsActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib/TwoIntsGoal.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef actionlib::TwoIntsGoal _goal_type; 23 | _goal_type goal; 24 | 25 | TwoIntsActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TwoIntsActionGoal"; }; 51 | const char * getMD5(){ return "684a2db55d6ffb8046fb9d6764ce0860"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TwoIntsActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsActionResult_h 2 | #define _ROS_actionlib_TwoIntsActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib/TwoIntsResult.h" 11 | 12 | namespace actionlib 13 | { 14 | 15 | class TwoIntsActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib::TwoIntsResult _result_type; 23 | _result_type result; 24 | 25 | TwoIntsActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib/TwoIntsActionResult"; }; 51 | const char * getMD5(){ return "3ba7dea8b8cddcae4528ade4ef74b6e7"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib/TwoIntsFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_TwoIntsFeedback_h 2 | #define _ROS_actionlib_TwoIntsFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib 10 | { 11 | 12 | class TwoIntsFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | TwoIntsFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "actionlib/TwoIntsFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/AveragingActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionFeedback_h 2 | #define _ROS_actionlib_tutorials_AveragingActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/AveragingFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib_tutorials::AveragingFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | AveragingActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/AveragingActionFeedback"; }; 51 | const char * getMD5(){ return "78a4a09241b1791069223ae7ebd5b16b"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/AveragingActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionGoal_h 2 | #define _ROS_actionlib_tutorials_AveragingActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib_tutorials/AveragingGoal.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef actionlib_tutorials::AveragingGoal _goal_type; 23 | _goal_type goal; 24 | 25 | AveragingActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/AveragingActionGoal"; }; 51 | const char * getMD5(){ return "1561825b734ebd6039851c501e3fb570"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/AveragingActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_AveragingActionResult_h 2 | #define _ROS_actionlib_tutorials_AveragingActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/AveragingResult.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class AveragingActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib_tutorials::AveragingResult _result_type; 23 | _result_type result; 24 | 25 | AveragingActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/AveragingActionResult"; }; 51 | const char * getMD5(){ return "8672cb489d347580acdcd05c5d497497"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/FibonacciActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionFeedback_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/FibonacciFeedback.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib_tutorials::FibonacciFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | FibonacciActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/FibonacciActionFeedback"; }; 51 | const char * getMD5(){ return "73b8497a9f629a31c0020900e4148f07"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/FibonacciActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionGoal_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "actionlib_tutorials/FibonacciGoal.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef actionlib_tutorials::FibonacciGoal _goal_type; 23 | _goal_type goal; 24 | 25 | FibonacciActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/FibonacciActionGoal"; }; 51 | const char * getMD5(){ return "006871c7fa1d0e3d5fe2226bf17b2a94"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/FibonacciActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciActionResult_h 2 | #define _ROS_actionlib_tutorials_FibonacciActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "actionlib_tutorials/FibonacciResult.h" 11 | 12 | namespace actionlib_tutorials 13 | { 14 | 15 | class FibonacciActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef actionlib_tutorials::FibonacciResult _result_type; 23 | _result_type result; 24 | 25 | FibonacciActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "actionlib_tutorials/FibonacciActionResult"; }; 51 | const char * getMD5(){ return "bee73a9fe29ae25e966e105f5553dd03"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/actionlib_tutorials/FibonacciGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_actionlib_tutorials_FibonacciGoal_h 2 | #define _ROS_actionlib_tutorials_FibonacciGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace actionlib_tutorials 10 | { 11 | 12 | class FibonacciGoal : public ros::Msg 13 | { 14 | public: 15 | typedef int32_t _order_type; 16 | _order_type order; 17 | 18 | FibonacciGoal(): 19 | order(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int32_t real; 28 | uint32_t base; 29 | } u_order; 30 | u_order.real = this->order; 31 | *(outbuffer + offset + 0) = (u_order.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_order.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_order.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_order.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->order); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | int32_t real; 44 | uint32_t base; 45 | } u_order; 46 | u_order.base = 0; 47 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_order.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->order = u_order.real; 52 | offset += sizeof(this->order); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "actionlib_tutorials/FibonacciGoal"; }; 57 | const char * getMD5(){ return "6889063349a00b249bd1661df429d822"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/bond/Constants.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_bond_Constants_h 2 | #define _ROS_bond_Constants_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace bond 10 | { 11 | 12 | class Constants : public ros::Msg 13 | { 14 | public: 15 | enum { DEAD_PUBLISH_PERIOD = 0.05 }; 16 | enum { DEFAULT_CONNECT_TIMEOUT = 10.0 }; 17 | enum { DEFAULT_HEARTBEAT_TIMEOUT = 4.0 }; 18 | enum { DEFAULT_DISCONNECT_TIMEOUT = 2.0 }; 19 | enum { DEFAULT_HEARTBEAT_PERIOD = 1.0 }; 20 | enum { DISABLE_HEARTBEAT_TIMEOUT_PARAM = /bond_disable_heartbeat_timeout }; 21 | 22 | Constants() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | virtual int deserialize(unsigned char *inbuffer) 33 | { 34 | int offset = 0; 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "bond/Constants"; }; 39 | const char * getMD5(){ return "6fc594dc1d7bd7919077042712f8c8b0"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/FollowJointTrajectoryActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionFeedback_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/FollowJointTrajectoryFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::FollowJointTrajectoryFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | FollowJointTrajectoryActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionFeedback"; }; 51 | const char * getMD5(){ return "d8920dc4eae9fc107e00999cce4be641"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/FollowJointTrajectoryActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionGoal_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/FollowJointTrajectoryGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef control_msgs::FollowJointTrajectoryGoal _goal_type; 23 | _goal_type goal; 24 | 25 | FollowJointTrajectoryActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionGoal"; }; 51 | const char * getMD5(){ return "cff5c1d533bf2f82dd0138d57f4304bb"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/FollowJointTrajectoryActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_FollowJointTrajectoryActionResult_h 2 | #define _ROS_control_msgs_FollowJointTrajectoryActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/FollowJointTrajectoryResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class FollowJointTrajectoryActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::FollowJointTrajectoryResult _result_type; 23 | _result_type result; 24 | 25 | FollowJointTrajectoryActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/FollowJointTrajectoryActionResult"; }; 51 | const char * getMD5(){ return "c4fb3b000dc9da4fd99699380efcc5d9"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/GripperCommandAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandAction_h 2 | #define _ROS_control_msgs_GripperCommandAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/GripperCommandActionGoal.h" 9 | #include "control_msgs/GripperCommandActionResult.h" 10 | #include "control_msgs/GripperCommandActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandAction : public ros::Msg 16 | { 17 | public: 18 | typedef control_msgs::GripperCommandActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef control_msgs::GripperCommandActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef control_msgs::GripperCommandActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | GripperCommandAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/GripperCommandAction"; }; 51 | const char * getMD5(){ return "950b2a6ebe831f5d4f4ceaba3d8be01e"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/GripperCommandActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionFeedback_h 2 | #define _ROS_control_msgs_GripperCommandActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/GripperCommandFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::GripperCommandFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | GripperCommandActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/GripperCommandActionFeedback"; }; 51 | const char * getMD5(){ return "653dff30c045f5e6ff3feb3409f4558d"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/GripperCommandActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionGoal_h 2 | #define _ROS_control_msgs_GripperCommandActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/GripperCommandGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef control_msgs::GripperCommandGoal _goal_type; 23 | _goal_type goal; 24 | 25 | GripperCommandActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/GripperCommandActionGoal"; }; 51 | const char * getMD5(){ return "aa581f648a35ed681db2ec0bf7a82bea"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/GripperCommandActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandActionResult_h 2 | #define _ROS_control_msgs_GripperCommandActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/GripperCommandResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class GripperCommandActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::GripperCommandResult _result_type; 23 | _result_type result; 24 | 25 | GripperCommandActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/GripperCommandActionResult"; }; 51 | const char * getMD5(){ return "143702cb2df0f163c5283cedc5efc6b6"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/GripperCommandGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommandGoal_h 2 | #define _ROS_control_msgs_GripperCommandGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/GripperCommand.h" 9 | 10 | namespace control_msgs 11 | { 12 | 13 | class GripperCommandGoal : public ros::Msg 14 | { 15 | public: 16 | typedef control_msgs::GripperCommand _command_type; 17 | _command_type command; 18 | 19 | GripperCommandGoal(): 20 | command() 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | offset += this->command.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->command.deserialize(inbuffer + offset); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "control_msgs/GripperCommandGoal"; }; 39 | const char * getMD5(){ return "86fd82f4ddc48a4cb6856cfa69217e43"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionFeedback_h 2 | #define _ROS_control_msgs_JointTrajectoryActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/JointTrajectoryFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::JointTrajectoryFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | JointTrajectoryActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/JointTrajectoryActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionGoal_h 2 | #define _ROS_control_msgs_JointTrajectoryActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/JointTrajectoryGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef control_msgs::JointTrajectoryGoal _goal_type; 23 | _goal_type goal; 24 | 25 | JointTrajectoryActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/JointTrajectoryActionGoal"; }; 51 | const char * getMD5(){ return "a99e83ef6185f9fdd7693efe99623a86"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryActionResult_h 2 | #define _ROS_control_msgs_JointTrajectoryActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/JointTrajectoryResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class JointTrajectoryActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::JointTrajectoryResult _result_type; 23 | _result_type result; 24 | 25 | JointTrajectoryActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/JointTrajectoryActionResult"; }; 51 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryFeedback_h 2 | #define _ROS_control_msgs_JointTrajectoryFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class JointTrajectoryFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | JointTrajectoryFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/JointTrajectoryFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryGoal_h 2 | #define _ROS_control_msgs_JointTrajectoryGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "trajectory_msgs/JointTrajectory.h" 9 | 10 | namespace control_msgs 11 | { 12 | 13 | class JointTrajectoryGoal : public ros::Msg 14 | { 15 | public: 16 | typedef trajectory_msgs::JointTrajectory _trajectory_type; 17 | _trajectory_type trajectory; 18 | 19 | JointTrajectoryGoal(): 20 | trajectory() 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | offset += this->trajectory.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->trajectory.deserialize(inbuffer + offset); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "control_msgs/JointTrajectoryGoal"; }; 39 | const char * getMD5(){ return "2a0eff76c870e8595636c2a562ca298e"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/JointTrajectoryResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_JointTrajectoryResult_h 2 | #define _ROS_control_msgs_JointTrajectoryResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class JointTrajectoryResult : public ros::Msg 13 | { 14 | public: 15 | 16 | JointTrajectoryResult() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/JointTrajectoryResult"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/PointHeadAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadAction_h 2 | #define _ROS_control_msgs_PointHeadAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "control_msgs/PointHeadActionGoal.h" 9 | #include "control_msgs/PointHeadActionResult.h" 10 | #include "control_msgs/PointHeadActionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadAction : public ros::Msg 16 | { 17 | public: 18 | typedef control_msgs::PointHeadActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef control_msgs::PointHeadActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef control_msgs::PointHeadActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | PointHeadAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/PointHeadAction"; }; 51 | const char * getMD5(){ return "7252920f1243de1b741f14f214125371"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/PointHeadActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionFeedback_h 2 | #define _ROS_control_msgs_PointHeadActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/PointHeadFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::PointHeadFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | PointHeadActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/PointHeadActionFeedback"; }; 51 | const char * getMD5(){ return "33c9244957176bbba97dd641119e8460"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/PointHeadActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionGoal_h 2 | #define _ROS_control_msgs_PointHeadActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/PointHeadGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef control_msgs::PointHeadGoal _goal_type; 23 | _goal_type goal; 24 | 25 | PointHeadActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/PointHeadActionGoal"; }; 51 | const char * getMD5(){ return "b53a8323d0ba7b310ba17a2d3a82a6b8"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/PointHeadActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadActionResult_h 2 | #define _ROS_control_msgs_PointHeadActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/PointHeadResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class PointHeadActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::PointHeadResult _result_type; 23 | _result_type result; 24 | 25 | PointHeadActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/PointHeadActionResult"; }; 51 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/PointHeadResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadResult_h 2 | #define _ROS_control_msgs_PointHeadResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class PointHeadResult : public ros::Msg 13 | { 14 | public: 15 | 16 | PointHeadResult() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/PointHeadResult"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/SingleJointPositionActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionFeedback_h 2 | #define _ROS_control_msgs_SingleJointPositionActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/SingleJointPositionFeedback.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::SingleJointPositionFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | SingleJointPositionActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/SingleJointPositionActionFeedback"; }; 51 | const char * getMD5(){ return "3503b7cf8972f90d245850a5d8796cfa"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/SingleJointPositionActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionGoal_h 2 | #define _ROS_control_msgs_SingleJointPositionActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "control_msgs/SingleJointPositionGoal.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef control_msgs::SingleJointPositionGoal _goal_type; 23 | _goal_type goal; 24 | 25 | SingleJointPositionActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/SingleJointPositionActionGoal"; }; 51 | const char * getMD5(){ return "4b0d3d091471663e17749c1d0db90f61"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/SingleJointPositionActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionActionResult_h 2 | #define _ROS_control_msgs_SingleJointPositionActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "control_msgs/SingleJointPositionResult.h" 11 | 12 | namespace control_msgs 13 | { 14 | 15 | class SingleJointPositionActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef control_msgs::SingleJointPositionResult _result_type; 23 | _result_type result; 24 | 25 | SingleJointPositionActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "control_msgs/SingleJointPositionActionResult"; }; 51 | const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/control_msgs/SingleJointPositionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_SingleJointPositionResult_h 2 | #define _ROS_control_msgs_SingleJointPositionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class SingleJointPositionResult : public ros::Msg 13 | { 14 | public: 15 | 16 | SingleJointPositionResult() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "control_msgs/SingleJointPositionResult"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/dynamic_reconfigure/SensorLevels.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_dynamic_reconfigure_SensorLevels_h 2 | #define _ROS_dynamic_reconfigure_SensorLevels_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace dynamic_reconfigure 10 | { 11 | 12 | class SensorLevels : public ros::Msg 13 | { 14 | public: 15 | enum { RECONFIGURE_CLOSE = 3 }; 16 | enum { RECONFIGURE_STOP = 1 }; 17 | enum { RECONFIGURE_RUNNING = 0 }; 18 | 19 | SensorLevels() 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | return offset; 27 | } 28 | 29 | virtual int deserialize(unsigned char *inbuffer) 30 | { 31 | int offset = 0; 32 | return offset; 33 | } 34 | 35 | const char * getType(){ return "dynamic_reconfigure/SensorLevels"; }; 36 | const char * getMD5(){ return "6322637bee96d5489db6e2127c47602c"; }; 37 | 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Accel.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Accel_h 2 | #define _ROS_geometry_msgs_Accel_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Accel : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Vector3 _linear_type; 17 | _linear_type linear; 18 | typedef geometry_msgs::Vector3 _angular_type; 19 | _angular_type angular; 20 | 21 | Accel(): 22 | linear(), 23 | angular() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->linear.serialize(outbuffer + offset); 31 | offset += this->angular.serialize(outbuffer + offset); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | offset += this->linear.deserialize(inbuffer + offset); 39 | offset += this->angular.deserialize(inbuffer + offset); 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "geometry_msgs/Accel"; }; 44 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif 50 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/AccelStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelStamped_h 2 | #define _ROS_geometry_msgs_AccelStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Accel.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Accel _accel_type; 20 | _accel_type accel; 21 | 22 | AccelStamped(): 23 | header(), 24 | accel() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->accel.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->accel.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/AccelStamped"; }; 45 | const char * getMD5(){ return "d8a98a5d81351b6eb0578c78557e7659"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/AccelWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_AccelWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/AccelWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class AccelWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::AccelWithCovariance _accel_type; 20 | _accel_type accel; 21 | 22 | AccelWithCovarianceStamped(): 23 | header(), 24 | accel() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->accel.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->accel.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/AccelWithCovarianceStamped"; }; 45 | const char * getMD5(){ return "96adb295225031ec8d57fb4251b0a886"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/InertiaStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_InertiaStamped_h 2 | #define _ROS_geometry_msgs_InertiaStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Inertia.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class InertiaStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Inertia _inertia_type; 20 | _inertia_type inertia; 21 | 22 | InertiaStamped(): 23 | header(), 24 | inertia() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->inertia.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->inertia.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/InertiaStamped"; }; 45 | const char * getMD5(){ return "ddee48caeab5a966c5e8d166654a9ac7"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/PointStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PointStamped_h 2 | #define _ROS_geometry_msgs_PointStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Point.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PointStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Point _point_type; 20 | _point_type point; 21 | 22 | PointStamped(): 23 | header(), 24 | point() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->point.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->point.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/PointStamped"; }; 45 | const char * getMD5(){ return "c63aecb41bfdfd6b7e1fac37c7cbe7bf"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/PolygonStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PolygonStamped_h 2 | #define _ROS_geometry_msgs_PolygonStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Polygon.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PolygonStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Polygon _polygon_type; 20 | _polygon_type polygon; 21 | 22 | PolygonStamped(): 23 | header(), 24 | polygon() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->polygon.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->polygon.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/PolygonStamped"; }; 45 | const char * getMD5(){ return "c6be8f7dc3bee7fe9e8d296070f53340"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Pose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Pose_h 2 | #define _ROS_geometry_msgs_Pose_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Point.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Pose : public ros::Msg 15 | { 16 | public: 17 | typedef geometry_msgs::Point _position_type; 18 | _position_type position; 19 | typedef geometry_msgs::Quaternion _orientation_type; 20 | _orientation_type orientation; 21 | 22 | Pose(): 23 | position(), 24 | orientation() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->position.serialize(outbuffer + offset); 32 | offset += this->orientation.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->position.deserialize(inbuffer + offset); 40 | offset += this->orientation.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/Pose"; }; 45 | const char * getMD5(){ return "e45d45a5a1ce597b249e23fb30fc871f"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/PoseStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseStamped_h 2 | #define _ROS_geometry_msgs_PoseStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Pose.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Pose _pose_type; 20 | _pose_type pose; 21 | 22 | PoseStamped(): 23 | header(), 24 | pose() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->pose.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->pose.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/PoseStamped"; }; 45 | const char * getMD5(){ return "d3812c3cbc69362b77dc0b19b345f8f5"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/PoseWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_PoseWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/PoseWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class PoseWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::PoseWithCovariance _pose_type; 20 | _pose_type pose; 21 | 22 | PoseWithCovarianceStamped(): 23 | header(), 24 | pose() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->pose.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->pose.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/PoseWithCovarianceStamped"; }; 45 | const char * getMD5(){ return "953b798c0f514ff060a53a3498ce6246"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/QuaternionStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_QuaternionStamped_h 2 | #define _ROS_geometry_msgs_QuaternionStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class QuaternionStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Quaternion _quaternion_type; 20 | _quaternion_type quaternion; 21 | 22 | QuaternionStamped(): 23 | header(), 24 | quaternion() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->quaternion.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->quaternion.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/QuaternionStamped"; }; 45 | const char * getMD5(){ return "e57f1e547e0e1fd13504588ffc8334e2"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Transform_h 2 | #define _ROS_geometry_msgs_Transform_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Transform : public ros::Msg 15 | { 16 | public: 17 | typedef geometry_msgs::Vector3 _translation_type; 18 | _translation_type translation; 19 | typedef geometry_msgs::Quaternion _rotation_type; 20 | _rotation_type rotation; 21 | 22 | Transform(): 23 | translation(), 24 | rotation() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->translation.serialize(outbuffer + offset); 32 | offset += this->rotation.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->translation.deserialize(inbuffer + offset); 40 | offset += this->rotation.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/Transform"; }; 45 | const char * getMD5(){ return "ac9eff44abf714214112b05d54a3cf9b"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Twist.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Twist_h 2 | #define _ROS_geometry_msgs_Twist_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Twist : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Vector3 _linear_type; 17 | _linear_type linear; 18 | typedef geometry_msgs::Vector3 _angular_type; 19 | _angular_type angular; 20 | 21 | Twist(): 22 | linear(), 23 | angular() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->linear.serialize(outbuffer + offset); 31 | offset += this->angular.serialize(outbuffer + offset); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | offset += this->linear.deserialize(inbuffer + offset); 39 | offset += this->angular.deserialize(inbuffer + offset); 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "geometry_msgs/Twist"; }; 44 | const char * getMD5(){ return "9f195f881246fdfa2798d1d3eebca84a"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif 50 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/TwistStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistStamped_h 2 | #define _ROS_geometry_msgs_TwistStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Twist.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Twist _twist_type; 20 | _twist_type twist; 21 | 22 | TwistStamped(): 23 | header(), 24 | twist() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->twist.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->twist.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/TwistStamped"; }; 45 | const char * getMD5(){ return "98d34b0043a2093cf9d9345ab6eef12e"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/TwistWithCovarianceStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovarianceStamped_h 2 | #define _ROS_geometry_msgs_TwistWithCovarianceStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/TwistWithCovariance.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class TwistWithCovarianceStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::TwistWithCovariance _twist_type; 20 | _twist_type twist; 21 | 22 | TwistWithCovarianceStamped(): 23 | header(), 24 | twist() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->twist.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->twist.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/TwistWithCovarianceStamped"; }; 45 | const char * getMD5(){ return "8927a1a12fb2607ceea095b2dc440a96"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Vector3Stamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Vector3Stamped_h 2 | #define _ROS_geometry_msgs_Vector3Stamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Vector3.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class Vector3Stamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Vector3 _vector_type; 20 | _vector_type vector; 21 | 22 | Vector3Stamped(): 23 | header(), 24 | vector() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->vector.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->vector.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/Vector3Stamped"; }; 45 | const char * getMD5(){ return "7b324c7325e683bf02a9b14b01090ec7"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/Wrench.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Wrench_h 2 | #define _ROS_geometry_msgs_Wrench_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Vector3.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class Wrench : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Vector3 _force_type; 17 | _force_type force; 18 | typedef geometry_msgs::Vector3 _torque_type; 19 | _torque_type torque; 20 | 21 | Wrench(): 22 | force(), 23 | torque() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->force.serialize(outbuffer + offset); 31 | offset += this->torque.serialize(outbuffer + offset); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | offset += this->force.deserialize(inbuffer + offset); 39 | offset += this->torque.deserialize(inbuffer + offset); 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "geometry_msgs/Wrench"; }; 44 | const char * getMD5(){ return "4f539cf138b23283b520fd271b567936"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif 50 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/geometry_msgs/WrenchStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_WrenchStamped_h 2 | #define _ROS_geometry_msgs_WrenchStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geometry_msgs/Wrench.h" 10 | 11 | namespace geometry_msgs 12 | { 13 | 14 | class WrenchStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geometry_msgs::Wrench _wrench_type; 20 | _wrench_type wrench; 21 | 22 | WrenchStamped(): 23 | header(), 24 | wrench() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->header.serialize(outbuffer + offset); 32 | offset += this->wrench.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->header.deserialize(inbuffer + offset); 40 | offset += this->wrench.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geometry_msgs/WrenchStamped"; }; 45 | const char * getMD5(){ return "d78d3cb249ce23087ade7e7d0c40cfa7"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/map_msgs/GetPointMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_GetPointMap_h 2 | #define _ROS_SERVICE_GetPointMap_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "sensor_msgs/PointCloud2.h" 8 | 9 | namespace map_msgs 10 | { 11 | 12 | static const char GETPOINTMAP[] = "map_msgs/GetPointMap"; 13 | 14 | class GetPointMapRequest : public ros::Msg 15 | { 16 | public: 17 | 18 | GetPointMapRequest() 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | return offset; 32 | } 33 | 34 | const char * getType(){ return GETPOINTMAP; }; 35 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 36 | 37 | }; 38 | 39 | class GetPointMapResponse : public ros::Msg 40 | { 41 | public: 42 | typedef sensor_msgs::PointCloud2 _map_type; 43 | _map_type map; 44 | 45 | GetPointMapResponse(): 46 | map() 47 | { 48 | } 49 | 50 | virtual int serialize(unsigned char *outbuffer) const 51 | { 52 | int offset = 0; 53 | offset += this->map.serialize(outbuffer + offset); 54 | return offset; 55 | } 56 | 57 | virtual int deserialize(unsigned char *inbuffer) 58 | { 59 | int offset = 0; 60 | offset += this->map.deserialize(inbuffer + offset); 61 | return offset; 62 | } 63 | 64 | const char * getType(){ return GETPOINTMAP; }; 65 | const char * getMD5(){ return "b84fbb39505086eb6a62d933c75cb7b4"; }; 66 | 67 | }; 68 | 69 | class GetPointMap { 70 | public: 71 | typedef GetPointMapRequest Request; 72 | typedef GetPointMapResponse Response; 73 | }; 74 | 75 | } 76 | #endif 77 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/map_msgs/SaveMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_SaveMap_h 2 | #define _ROS_SERVICE_SaveMap_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "std_msgs/String.h" 8 | 9 | namespace map_msgs 10 | { 11 | 12 | static const char SAVEMAP[] = "map_msgs/SaveMap"; 13 | 14 | class SaveMapRequest : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::String _filename_type; 18 | _filename_type filename; 19 | 20 | SaveMapRequest(): 21 | filename() 22 | { 23 | } 24 | 25 | virtual int serialize(unsigned char *outbuffer) const 26 | { 27 | int offset = 0; 28 | offset += this->filename.serialize(outbuffer + offset); 29 | return offset; 30 | } 31 | 32 | virtual int deserialize(unsigned char *inbuffer) 33 | { 34 | int offset = 0; 35 | offset += this->filename.deserialize(inbuffer + offset); 36 | return offset; 37 | } 38 | 39 | const char * getType(){ return SAVEMAP; }; 40 | const char * getMD5(){ return "716e25f9d9dc76ceba197f93cbf05dc7"; }; 41 | 42 | }; 43 | 44 | class SaveMapResponse : public ros::Msg 45 | { 46 | public: 47 | 48 | SaveMapResponse() 49 | { 50 | } 51 | 52 | virtual int serialize(unsigned char *outbuffer) const 53 | { 54 | int offset = 0; 55 | return offset; 56 | } 57 | 58 | virtual int deserialize(unsigned char *inbuffer) 59 | { 60 | int offset = 0; 61 | return offset; 62 | } 63 | 64 | const char * getType(){ return SAVEMAP; }; 65 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 66 | 67 | }; 68 | 69 | class SaveMap { 70 | public: 71 | typedef SaveMapRequest Request; 72 | typedef SaveMapResponse Response; 73 | }; 74 | 75 | } 76 | #endif 77 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_GetMap_h 2 | #define _ROS_SERVICE_GetMap_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | #include "nav_msgs/OccupancyGrid.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | static const char GETMAP[] = "nav_msgs/GetMap"; 13 | 14 | class GetMapRequest : public ros::Msg 15 | { 16 | public: 17 | 18 | GetMapRequest() 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | return offset; 26 | } 27 | 28 | virtual int deserialize(unsigned char *inbuffer) 29 | { 30 | int offset = 0; 31 | return offset; 32 | } 33 | 34 | const char * getType(){ return GETMAP; }; 35 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 36 | 37 | }; 38 | 39 | class GetMapResponse : public ros::Msg 40 | { 41 | public: 42 | typedef nav_msgs::OccupancyGrid _map_type; 43 | _map_type map; 44 | 45 | GetMapResponse(): 46 | map() 47 | { 48 | } 49 | 50 | virtual int serialize(unsigned char *outbuffer) const 51 | { 52 | int offset = 0; 53 | offset += this->map.serialize(outbuffer + offset); 54 | return offset; 55 | } 56 | 57 | virtual int deserialize(unsigned char *inbuffer) 58 | { 59 | int offset = 0; 60 | offset += this->map.deserialize(inbuffer + offset); 61 | return offset; 62 | } 63 | 64 | const char * getType(){ return GETMAP; }; 65 | const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; 66 | 67 | }; 68 | 69 | class GetMap { 70 | public: 71 | typedef GetMapRequest Request; 72 | typedef GetMapResponse Response; 73 | }; 74 | 75 | } 76 | #endif 77 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapAction_h 2 | #define _ROS_nav_msgs_GetMapAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "nav_msgs/GetMapActionGoal.h" 9 | #include "nav_msgs/GetMapActionResult.h" 10 | #include "nav_msgs/GetMapActionFeedback.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapAction : public ros::Msg 16 | { 17 | public: 18 | typedef nav_msgs::GetMapActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef nav_msgs::GetMapActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef nav_msgs::GetMapActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | GetMapAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "nav_msgs/GetMapAction"; }; 51 | const char * getMD5(){ return "e611ad23fbf237c031b7536416dc7cd7"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionFeedback_h 2 | #define _ROS_nav_msgs_GetMapActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "nav_msgs/GetMapFeedback.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef nav_msgs::GetMapFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | GetMapActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "nav_msgs/GetMapActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionGoal_h 2 | #define _ROS_nav_msgs_GetMapActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "nav_msgs/GetMapGoal.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef nav_msgs::GetMapGoal _goal_type; 23 | _goal_type goal; 24 | 25 | GetMapActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "nav_msgs/GetMapActionGoal"; }; 51 | const char * getMD5(){ return "4b30be6cd12b9e72826df56b481f40e0"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapActionResult_h 2 | #define _ROS_nav_msgs_GetMapActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "nav_msgs/GetMapResult.h" 11 | 12 | namespace nav_msgs 13 | { 14 | 15 | class GetMapActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef nav_msgs::GetMapResult _result_type; 23 | _result_type result; 24 | 25 | GetMapActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "nav_msgs/GetMapActionResult"; }; 51 | const char * getMD5(){ return "ac66e5b9a79bb4bbd33dab245236c892"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapFeedback_h 2 | #define _ROS_nav_msgs_GetMapFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | class GetMapFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | GetMapFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "nav_msgs/GetMapFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapGoal_h 2 | #define _ROS_nav_msgs_GetMapGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace nav_msgs 10 | { 11 | 12 | class GetMapGoal : public ros::Msg 13 | { 14 | public: 15 | 16 | GetMapGoal() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "nav_msgs/GetMapGoal"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/nav_msgs/GetMapResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_nav_msgs_GetMapResult_h 2 | #define _ROS_nav_msgs_GetMapResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "nav_msgs/OccupancyGrid.h" 9 | 10 | namespace nav_msgs 11 | { 12 | 13 | class GetMapResult : public ros::Msg 14 | { 15 | public: 16 | typedef nav_msgs::OccupancyGrid _map_type; 17 | _map_type map; 18 | 19 | GetMapResult(): 20 | map() 21 | { 22 | } 23 | 24 | virtual int serialize(unsigned char *outbuffer) const 25 | { 26 | int offset = 0; 27 | offset += this->map.serialize(outbuffer + offset); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | offset += this->map.deserialize(inbuffer + offset); 35 | return offset; 36 | } 37 | 38 | const char * getType(){ return "nav_msgs/GetMapResult"; }; 39 | const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; 40 | 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/roscpp/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Empty_h 2 | #define _ROS_SERVICE_Empty_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace roscpp 9 | { 10 | 11 | static const char EMPTY[] = "roscpp/Empty"; 12 | 13 | class EmptyRequest : public ros::Msg 14 | { 15 | public: 16 | 17 | EmptyRequest() 18 | { 19 | } 20 | 21 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | return offset; 25 | } 26 | 27 | virtual int deserialize(unsigned char *inbuffer) 28 | { 29 | int offset = 0; 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return EMPTY; }; 34 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 35 | 36 | }; 37 | 38 | class EmptyResponse : public ros::Msg 39 | { 40 | public: 41 | 42 | EmptyResponse() 43 | { 44 | } 45 | 46 | virtual int serialize(unsigned char *outbuffer) const 47 | { 48 | int offset = 0; 49 | return offset; 50 | } 51 | 52 | virtual int deserialize(unsigned char *inbuffer) 53 | { 54 | int offset = 0; 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return EMPTY; }; 59 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 60 | 61 | }; 62 | 63 | class Empty { 64 | public: 65 | typedef EmptyRequest Request; 66 | typedef EmptyResponse Response; 67 | }; 68 | 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/rospy_tutorials/HeaderString.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rospy_tutorials_HeaderString_h 2 | #define _ROS_rospy_tutorials_HeaderString_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace rospy_tutorials 11 | { 12 | 13 | class HeaderString : public ros::Msg 14 | { 15 | public: 16 | typedef std_msgs::Header _header_type; 17 | _header_type header; 18 | typedef const char* _data_type; 19 | _data_type data; 20 | 21 | HeaderString(): 22 | header(), 23 | data("") 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->header.serialize(outbuffer + offset); 31 | uint32_t length_data = strlen(this->data); 32 | varToArr(outbuffer + offset, length_data); 33 | offset += 4; 34 | memcpy(outbuffer + offset, this->data, length_data); 35 | offset += length_data; 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | offset += this->header.deserialize(inbuffer + offset); 43 | uint32_t length_data; 44 | arrToVar(length_data, (inbuffer + offset)); 45 | offset += 4; 46 | for(unsigned int k= offset; k< offset+length_data; ++k){ 47 | inbuffer[k-1]=inbuffer[k]; 48 | } 49 | inbuffer[offset+length_data-1]=0; 50 | this->data = (char *)(inbuffer + offset-1); 51 | offset += length_data; 52 | return offset; 53 | } 54 | 55 | const char * getType(){ return "rospy_tutorials/HeaderString"; }; 56 | const char * getMD5(){ return "c99a9440709e4d4a9716d55b8270d5e7"; }; 57 | 58 | }; 59 | 60 | } 61 | #endif 62 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/rosserial_msgs/Log.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_rosserial_msgs_Log_h 2 | #define _ROS_rosserial_msgs_Log_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace rosserial_msgs 10 | { 11 | 12 | class Log : public ros::Msg 13 | { 14 | public: 15 | typedef uint8_t _level_type; 16 | _level_type level; 17 | typedef const char* _msg_type; 18 | _msg_type msg; 19 | enum { ROSDEBUG = 0 }; 20 | enum { INFO = 1 }; 21 | enum { WARN = 2 }; 22 | enum { ERROR = 3 }; 23 | enum { FATAL = 4 }; 24 | 25 | Log(): 26 | level(0), 27 | msg("") 28 | { 29 | } 30 | 31 | virtual int serialize(unsigned char *outbuffer) const 32 | { 33 | int offset = 0; 34 | *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; 35 | offset += sizeof(this->level); 36 | uint32_t length_msg = strlen(this->msg); 37 | varToArr(outbuffer + offset, length_msg); 38 | offset += 4; 39 | memcpy(outbuffer + offset, this->msg, length_msg); 40 | offset += length_msg; 41 | return offset; 42 | } 43 | 44 | virtual int deserialize(unsigned char *inbuffer) 45 | { 46 | int offset = 0; 47 | this->level = ((uint8_t) (*(inbuffer + offset))); 48 | offset += sizeof(this->level); 49 | uint32_t length_msg; 50 | arrToVar(length_msg, (inbuffer + offset)); 51 | offset += 4; 52 | for(unsigned int k= offset; k< offset+length_msg; ++k){ 53 | inbuffer[k-1]=inbuffer[k]; 54 | } 55 | inbuffer[offset+length_msg-1]=0; 56 | this->msg = (char *)(inbuffer + offset-1); 57 | offset += length_msg; 58 | return offset; 59 | } 60 | 61 | const char * getType(){ return "rosserial_msgs/Log"; }; 62 | const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; 63 | 64 | }; 65 | 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/shape_msgs/MeshTriangle.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_shape_msgs_MeshTriangle_h 2 | #define _ROS_shape_msgs_MeshTriangle_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace shape_msgs 10 | { 11 | 12 | class MeshTriangle : public ros::Msg 13 | { 14 | public: 15 | uint32_t vertex_indices[3]; 16 | 17 | MeshTriangle(): 18 | vertex_indices() 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | for( uint32_t i = 0; i < 3; i++){ 26 | *(outbuffer + offset + 0) = (this->vertex_indices[i] >> (8 * 0)) & 0xFF; 27 | *(outbuffer + offset + 1) = (this->vertex_indices[i] >> (8 * 1)) & 0xFF; 28 | *(outbuffer + offset + 2) = (this->vertex_indices[i] >> (8 * 2)) & 0xFF; 29 | *(outbuffer + offset + 3) = (this->vertex_indices[i] >> (8 * 3)) & 0xFF; 30 | offset += sizeof(this->vertex_indices[i]); 31 | } 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | for( uint32_t i = 0; i < 3; i++){ 39 | this->vertex_indices[i] = ((uint32_t) (*(inbuffer + offset))); 40 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 41 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 42 | this->vertex_indices[i] |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 43 | offset += sizeof(this->vertex_indices[i]); 44 | } 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "shape_msgs/MeshTriangle"; }; 49 | const char * getMD5(){ return "23688b2e6d2de3d32fe8af104a903253"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Bool.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Bool_h 2 | #define _ROS_std_msgs_Bool_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Bool : public ros::Msg 13 | { 14 | public: 15 | typedef bool _data_type; 16 | _data_type data; 17 | 18 | Bool(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | bool real; 28 | uint8_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | bool real; 41 | uint8_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/Bool"; }; 51 | const char * getMD5(){ return "8b94c1b53db61fb6aed406028ad6332a"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Byte.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Byte_h 2 | #define _ROS_std_msgs_Byte_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Byte : public ros::Msg 13 | { 14 | public: 15 | typedef int8_t _data_type; 16 | _data_type data; 17 | 18 | Byte(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int8_t real; 28 | uint8_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | int8_t real; 41 | uint8_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/Byte"; }; 51 | const char * getMD5(){ return "ad736a2e8818154c487bb80fe42ce43b"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Char.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Char_h 2 | #define _ROS_std_msgs_Char_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Char : public ros::Msg 13 | { 14 | public: 15 | typedef uint8_t _data_type; 16 | _data_type data; 17 | 18 | Char(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 27 | offset += sizeof(this->data); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->data = ((uint8_t) (*(inbuffer + offset))); 35 | offset += sizeof(this->data); 36 | return offset; 37 | } 38 | 39 | const char * getType(){ return "std_msgs/Char"; }; 40 | const char * getMD5(){ return "1bf77f25acecdedba0e224b162199717"; }; 41 | 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Empty_h 2 | #define _ROS_std_msgs_Empty_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Empty : public ros::Msg 13 | { 14 | public: 15 | 16 | Empty() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "std_msgs/Empty"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Float32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float32_h 2 | #define _ROS_std_msgs_Float32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float32 : public ros::Msg 13 | { 14 | public: 15 | typedef float _data_type; 16 | _data_type data; 17 | 18 | Float32(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | float real; 28 | uint32_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->data); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | float real; 44 | uint32_t base; 45 | } u_data; 46 | u_data.base = 0; 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->data = u_data.real; 52 | offset += sizeof(this->data); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "std_msgs/Float32"; }; 57 | const char * getMD5(){ return "73fcbf46b49191e672908e50842a83d4"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Int16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int16_h 2 | #define _ROS_std_msgs_Int16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int16 : public ros::Msg 13 | { 14 | public: 15 | typedef int16_t _data_type; 16 | _data_type data; 17 | 18 | Int16(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int16_t real; 28 | uint16_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 33 | offset += sizeof(this->data); 34 | return offset; 35 | } 36 | 37 | virtual int deserialize(unsigned char *inbuffer) 38 | { 39 | int offset = 0; 40 | union { 41 | int16_t real; 42 | uint16_t base; 43 | } u_data; 44 | u_data.base = 0; 45 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); 46 | u_data.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 47 | this->data = u_data.real; 48 | offset += sizeof(this->data); 49 | return offset; 50 | } 51 | 52 | const char * getType(){ return "std_msgs/Int16"; }; 53 | const char * getMD5(){ return "8524586e34fbd7cb1c08c5f5f1ca0e57"; }; 54 | 55 | }; 56 | 57 | } 58 | #endif 59 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Int32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int32_h 2 | #define _ROS_std_msgs_Int32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int32 : public ros::Msg 13 | { 14 | public: 15 | typedef int32_t _data_type; 16 | _data_type data; 17 | 18 | Int32(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int32_t real; 28 | uint32_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | *(outbuffer + offset + 1) = (u_data.base >> (8 * 1)) & 0xFF; 33 | *(outbuffer + offset + 2) = (u_data.base >> (8 * 2)) & 0xFF; 34 | *(outbuffer + offset + 3) = (u_data.base >> (8 * 3)) & 0xFF; 35 | offset += sizeof(this->data); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | union { 43 | int32_t real; 44 | uint32_t base; 45 | } u_data; 46 | u_data.base = 0; 47 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 48 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 49 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 50 | u_data.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 51 | this->data = u_data.real; 52 | offset += sizeof(this->data); 53 | return offset; 54 | } 55 | 56 | const char * getType(){ return "std_msgs/Int32"; }; 57 | const char * getMD5(){ return "da5909fbe378aeaf85e547e830cc1bb7"; }; 58 | 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/Int8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Int8_h 2 | #define _ROS_std_msgs_Int8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Int8 : public ros::Msg 13 | { 14 | public: 15 | typedef int8_t _data_type; 16 | _data_type data; 17 | 18 | Int8(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | union { 27 | int8_t real; 28 | uint8_t base; 29 | } u_data; 30 | u_data.real = this->data; 31 | *(outbuffer + offset + 0) = (u_data.base >> (8 * 0)) & 0xFF; 32 | offset += sizeof(this->data); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | union { 40 | int8_t real; 41 | uint8_t base; 42 | } u_data; 43 | u_data.base = 0; 44 | u_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 45 | this->data = u_data.real; 46 | offset += sizeof(this->data); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "std_msgs/Int8"; }; 51 | const char * getMD5(){ return "27ffa0c9c4b8fb8492252bcad9e5c57b"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/String.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_String_h 2 | #define _ROS_std_msgs_String_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class String : public ros::Msg 13 | { 14 | public: 15 | typedef const char* _data_type; 16 | _data_type data; 17 | 18 | String(): 19 | data("") 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | uint32_t length_data = strlen(this->data); 27 | varToArr(outbuffer + offset, length_data); 28 | offset += 4; 29 | memcpy(outbuffer + offset, this->data, length_data); 30 | offset += length_data; 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | uint32_t length_data; 38 | arrToVar(length_data, (inbuffer + offset)); 39 | offset += 4; 40 | for(unsigned int k= offset; k< offset+length_data; ++k){ 41 | inbuffer[k-1]=inbuffer[k]; 42 | } 43 | inbuffer[offset+length_data-1]=0; 44 | this->data = (char *)(inbuffer + offset-1); 45 | offset += length_data; 46 | return offset; 47 | } 48 | 49 | const char * getType(){ return "std_msgs/String"; }; 50 | const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; }; 51 | 52 | }; 53 | 54 | } 55 | #endif 56 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/UInt16.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt16_h 2 | #define _ROS_std_msgs_UInt16_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt16 : public ros::Msg 13 | { 14 | public: 15 | typedef uint16_t _data_type; 16 | _data_type data; 17 | 18 | UInt16(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 27 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 28 | offset += sizeof(this->data); 29 | return offset; 30 | } 31 | 32 | virtual int deserialize(unsigned char *inbuffer) 33 | { 34 | int offset = 0; 35 | this->data = ((uint16_t) (*(inbuffer + offset))); 36 | this->data |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); 37 | offset += sizeof(this->data); 38 | return offset; 39 | } 40 | 41 | const char * getType(){ return "std_msgs/UInt16"; }; 42 | const char * getMD5(){ return "1df79edf208b629fe6b81923a544552d"; }; 43 | 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/UInt32.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt32_h 2 | #define _ROS_std_msgs_UInt32_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt32 : public ros::Msg 13 | { 14 | public: 15 | typedef uint32_t _data_type; 16 | _data_type data; 17 | 18 | UInt32(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 27 | *(outbuffer + offset + 1) = (this->data >> (8 * 1)) & 0xFF; 28 | *(outbuffer + offset + 2) = (this->data >> (8 * 2)) & 0xFF; 29 | *(outbuffer + offset + 3) = (this->data >> (8 * 3)) & 0xFF; 30 | offset += sizeof(this->data); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | this->data = ((uint32_t) (*(inbuffer + offset))); 38 | this->data |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 39 | this->data |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 40 | this->data |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 41 | offset += sizeof(this->data); 42 | return offset; 43 | } 44 | 45 | const char * getType(){ return "std_msgs/UInt32"; }; 46 | const char * getMD5(){ return "304a39449588c7f8ce2df6e8001c5fce"; }; 47 | 48 | }; 49 | 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_msgs/UInt8.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_UInt8_h 2 | #define _ROS_std_msgs_UInt8_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class UInt8 : public ros::Msg 13 | { 14 | public: 15 | typedef uint8_t _data_type; 16 | _data_type data; 17 | 18 | UInt8(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | *(outbuffer + offset + 0) = (this->data >> (8 * 0)) & 0xFF; 27 | offset += sizeof(this->data); 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | this->data = ((uint8_t) (*(inbuffer + offset))); 35 | offset += sizeof(this->data); 36 | return offset; 37 | } 38 | 39 | const char * getType(){ return "std_msgs/UInt8"; }; 40 | const char * getMD5(){ return "7c8164229e7d2c17eb95e9231617fdee"; }; 41 | 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/std_srvs/Empty.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_SERVICE_Empty_h 2 | #define _ROS_SERVICE_Empty_h 3 | #include 4 | #include 5 | #include 6 | #include "ros/msg.h" 7 | 8 | namespace std_srvs 9 | { 10 | 11 | static const char EMPTY[] = "std_srvs/Empty"; 12 | 13 | class EmptyRequest : public ros::Msg 14 | { 15 | public: 16 | 17 | EmptyRequest() 18 | { 19 | } 20 | 21 | virtual int serialize(unsigned char *outbuffer) const 22 | { 23 | int offset = 0; 24 | return offset; 25 | } 26 | 27 | virtual int deserialize(unsigned char *inbuffer) 28 | { 29 | int offset = 0; 30 | return offset; 31 | } 32 | 33 | const char * getType(){ return EMPTY; }; 34 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 35 | 36 | }; 37 | 38 | class EmptyResponse : public ros::Msg 39 | { 40 | public: 41 | 42 | EmptyResponse() 43 | { 44 | } 45 | 46 | virtual int serialize(unsigned char *outbuffer) const 47 | { 48 | int offset = 0; 49 | return offset; 50 | } 51 | 52 | virtual int deserialize(unsigned char *inbuffer) 53 | { 54 | int offset = 0; 55 | return offset; 56 | } 57 | 58 | const char * getType(){ return EMPTY; }; 59 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 60 | 61 | }; 62 | 63 | class Empty { 64 | public: 65 | typedef EmptyRequest Request; 66 | typedef EmptyResponse Response; 67 | }; 68 | 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformAction_h 2 | #define _ROS_tf2_msgs_LookupTransformAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "tf2_msgs/LookupTransformActionGoal.h" 9 | #include "tf2_msgs/LookupTransformActionResult.h" 10 | #include "tf2_msgs/LookupTransformActionFeedback.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformAction : public ros::Msg 16 | { 17 | public: 18 | typedef tf2_msgs::LookupTransformActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef tf2_msgs::LookupTransformActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef tf2_msgs::LookupTransformActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | LookupTransformAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "tf2_msgs/LookupTransformAction"; }; 51 | const char * getMD5(){ return "7ee01ba91a56c2245c610992dbaa3c37"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionFeedback_h 2 | #define _ROS_tf2_msgs_LookupTransformActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "tf2_msgs/LookupTransformFeedback.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef tf2_msgs::LookupTransformFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | LookupTransformActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "tf2_msgs/LookupTransformActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionGoal_h 2 | #define _ROS_tf2_msgs_LookupTransformActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "tf2_msgs/LookupTransformGoal.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef tf2_msgs::LookupTransformGoal _goal_type; 23 | _goal_type goal; 24 | 25 | LookupTransformActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "tf2_msgs/LookupTransformActionGoal"; }; 51 | const char * getMD5(){ return "f2e7bcdb75c847978d0351a13e699da5"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformActionResult_h 2 | #define _ROS_tf2_msgs_LookupTransformActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "tf2_msgs/LookupTransformResult.h" 11 | 12 | namespace tf2_msgs 13 | { 14 | 15 | class LookupTransformActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef tf2_msgs::LookupTransformResult _result_type; 23 | _result_type result; 24 | 25 | LookupTransformActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "tf2_msgs/LookupTransformActionResult"; }; 51 | const char * getMD5(){ return "ac26ce75a41384fa8bb4dc10f491ab90"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformFeedback_h 2 | #define _ROS_tf2_msgs_LookupTransformFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace tf2_msgs 10 | { 11 | 12 | class LookupTransformFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | LookupTransformFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "tf2_msgs/LookupTransformFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/tf2_msgs/LookupTransformResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_tf2_msgs_LookupTransformResult_h 2 | #define _ROS_tf2_msgs_LookupTransformResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/TransformStamped.h" 9 | #include "tf2_msgs/TF2Error.h" 10 | 11 | namespace tf2_msgs 12 | { 13 | 14 | class LookupTransformResult : public ros::Msg 15 | { 16 | public: 17 | typedef geometry_msgs::TransformStamped _transform_type; 18 | _transform_type transform; 19 | typedef tf2_msgs::TF2Error _error_type; 20 | _error_type error; 21 | 22 | LookupTransformResult(): 23 | transform(), 24 | error() 25 | { 26 | } 27 | 28 | virtual int serialize(unsigned char *outbuffer) const 29 | { 30 | int offset = 0; 31 | offset += this->transform.serialize(outbuffer + offset); 32 | offset += this->error.serialize(outbuffer + offset); 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->transform.deserialize(inbuffer + offset); 40 | offset += this->error.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "tf2_msgs/LookupTransformResult"; }; 45 | const char * getMD5(){ return "3fe5db6a19ca9cfb675418c5ad875c36"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtle_actionlib/ShapeAction.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeAction_h 2 | #define _ROS_turtle_actionlib_ShapeAction_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "turtle_actionlib/ShapeActionGoal.h" 9 | #include "turtle_actionlib/ShapeActionResult.h" 10 | #include "turtle_actionlib/ShapeActionFeedback.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeAction : public ros::Msg 16 | { 17 | public: 18 | typedef turtle_actionlib::ShapeActionGoal _action_goal_type; 19 | _action_goal_type action_goal; 20 | typedef turtle_actionlib::ShapeActionResult _action_result_type; 21 | _action_result_type action_result; 22 | typedef turtle_actionlib::ShapeActionFeedback _action_feedback_type; 23 | _action_feedback_type action_feedback; 24 | 25 | ShapeAction(): 26 | action_goal(), 27 | action_result(), 28 | action_feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->action_goal.serialize(outbuffer + offset); 36 | offset += this->action_result.serialize(outbuffer + offset); 37 | offset += this->action_feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->action_goal.deserialize(inbuffer + offset); 45 | offset += this->action_result.deserialize(inbuffer + offset); 46 | offset += this->action_feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "turtle_actionlib/ShapeAction"; }; 51 | const char * getMD5(){ return "d73b17d6237a925511f5d7727a1dc903"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtle_actionlib/ShapeActionFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionFeedback_h 2 | #define _ROS_turtle_actionlib_ShapeActionFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "turtle_actionlib/ShapeFeedback.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionFeedback : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef turtle_actionlib::ShapeFeedback _feedback_type; 23 | _feedback_type feedback; 24 | 25 | ShapeActionFeedback(): 26 | header(), 27 | status(), 28 | feedback() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->feedback.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->feedback.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "turtle_actionlib/ShapeActionFeedback"; }; 51 | const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtle_actionlib/ShapeActionGoal.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionGoal_h 2 | #define _ROS_turtle_actionlib_ShapeActionGoal_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalID.h" 10 | #include "turtle_actionlib/ShapeGoal.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionGoal : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalID _goal_id_type; 21 | _goal_id_type goal_id; 22 | typedef turtle_actionlib::ShapeGoal _goal_type; 23 | _goal_type goal; 24 | 25 | ShapeActionGoal(): 26 | header(), 27 | goal_id(), 28 | goal() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->goal_id.serialize(outbuffer + offset); 37 | offset += this->goal.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->goal_id.deserialize(inbuffer + offset); 46 | offset += this->goal.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "turtle_actionlib/ShapeActionGoal"; }; 51 | const char * getMD5(){ return "dbfccd187f2ec9c593916447ffd6cc77"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtle_actionlib/ShapeActionResult.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeActionResult_h 2 | #define _ROS_turtle_actionlib_ShapeActionResult_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "actionlib_msgs/GoalStatus.h" 10 | #include "turtle_actionlib/ShapeResult.h" 11 | 12 | namespace turtle_actionlib 13 | { 14 | 15 | class ShapeActionResult : public ros::Msg 16 | { 17 | public: 18 | typedef std_msgs::Header _header_type; 19 | _header_type header; 20 | typedef actionlib_msgs::GoalStatus _status_type; 21 | _status_type status; 22 | typedef turtle_actionlib::ShapeResult _result_type; 23 | _result_type result; 24 | 25 | ShapeActionResult(): 26 | header(), 27 | status(), 28 | result() 29 | { 30 | } 31 | 32 | virtual int serialize(unsigned char *outbuffer) const 33 | { 34 | int offset = 0; 35 | offset += this->header.serialize(outbuffer + offset); 36 | offset += this->status.serialize(outbuffer + offset); 37 | offset += this->result.serialize(outbuffer + offset); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | offset += this->header.deserialize(inbuffer + offset); 45 | offset += this->status.deserialize(inbuffer + offset); 46 | offset += this->result.deserialize(inbuffer + offset); 47 | return offset; 48 | } 49 | 50 | const char * getType(){ return "turtle_actionlib/ShapeActionResult"; }; 51 | const char * getMD5(){ return "c8d13d5d140f1047a2e4d3bf5c045822"; }; 52 | 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtle_actionlib/ShapeFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtle_actionlib_ShapeFeedback_h 2 | #define _ROS_turtle_actionlib_ShapeFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace turtle_actionlib 10 | { 11 | 12 | class ShapeFeedback : public ros::Msg 13 | { 14 | public: 15 | 16 | ShapeFeedback() 17 | { 18 | } 19 | 20 | virtual int serialize(unsigned char *outbuffer) const 21 | { 22 | int offset = 0; 23 | return offset; 24 | } 25 | 26 | virtual int deserialize(unsigned char *inbuffer) 27 | { 28 | int offset = 0; 29 | return offset; 30 | } 31 | 32 | const char * getType(){ return "turtle_actionlib/ShapeFeedback"; }; 33 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; 34 | 35 | }; 36 | 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /llc-teensy/lib/ros_lib/turtlesim/Color.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_turtlesim_Color_h 2 | #define _ROS_turtlesim_Color_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace turtlesim 10 | { 11 | 12 | class Color : public ros::Msg 13 | { 14 | public: 15 | typedef uint8_t _r_type; 16 | _r_type r; 17 | typedef uint8_t _g_type; 18 | _g_type g; 19 | typedef uint8_t _b_type; 20 | _b_type b; 21 | 22 | Color(): 23 | r(0), 24 | g(0), 25 | b(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | *(outbuffer + offset + 0) = (this->r >> (8 * 0)) & 0xFF; 33 | offset += sizeof(this->r); 34 | *(outbuffer + offset + 0) = (this->g >> (8 * 0)) & 0xFF; 35 | offset += sizeof(this->g); 36 | *(outbuffer + offset + 0) = (this->b >> (8 * 0)) & 0xFF; 37 | offset += sizeof(this->b); 38 | return offset; 39 | } 40 | 41 | virtual int deserialize(unsigned char *inbuffer) 42 | { 43 | int offset = 0; 44 | this->r = ((uint8_t) (*(inbuffer + offset))); 45 | offset += sizeof(this->r); 46 | this->g = ((uint8_t) (*(inbuffer + offset))); 47 | offset += sizeof(this->g); 48 | this->b = ((uint8_t) (*(inbuffer + offset))); 49 | offset += sizeof(this->b); 50 | return offset; 51 | } 52 | 53 | const char * getType(){ return "turtlesim/Color"; }; 54 | const char * getMD5(){ return "353891e354491c51aabe32df673fb446"; }; 55 | 56 | }; 57 | 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /llc-teensy/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:teensy40] 12 | platform = teensy 13 | board = teensy40 14 | framework = arduino 15 | upload_port = /dev/cu.usbmodem75602101 16 | -------------------------------------------------------------------------------- /llc-teensy/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /media/OpenQuadruped Node Infrastructure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/OpenQuadruped Node Infrastructure.png -------------------------------------------------------------------------------- /media/OpenQuadruped.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/OpenQuadruped.png -------------------------------------------------------------------------------- /media/SideView.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/SideView.png -------------------------------------------------------------------------------- /media/bodyik_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/bodyik_demo.gif -------------------------------------------------------------------------------- /media/trot_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/trot_demo.gif -------------------------------------------------------------------------------- /media/wiring_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/media/wiring_diagram.png -------------------------------------------------------------------------------- /ros-workspace/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /ros-workspace/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/launch/real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/launch/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/msg/JointAngles.msg: -------------------------------------------------------------------------------- 1 | float32[] fl 2 | float32[] fr 3 | float32[] bl 4 | float32[] br 5 | -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/nodes/\: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy, time, math, cv2, sys 4 | 5 | from std_msgs.msg import String, Float32, Image, LaserScan, Int32 6 | from sensor_msgs.msg import Joy 7 | 8 | body_ik = 0 9 | gait_planner = 1 10 | 11 | mode = body_ik 12 | 13 | #define function/functions to provide the required functionality 14 | def fnc_callback(msg): 15 | global mode 16 | print(msg.data) 17 | 18 | if __name__=='__main__': 19 | #Add here the name of the ROS. In ROS, names are unique named. 20 | rospy.init_node('interface_process') 21 | #subscribe to a topic using rospy.Subscriber class 22 | sub=rospy.Subscriber('joy', Joy, fnc_callback) 23 | #publish messages to a topic using rospy.Publisher class 24 | # pub=rospy.Publisher('joint_angles', THE_TYPE_OF_THE_MESSAGE, queue_size=1) 25 | # rate=rospy.Rate(300) # puvlish rate: 300 hz 26 | # 27 | # while not rospy.is_shutdown(): 28 | # if varS<= var2: 29 | # varP=something() 30 | # else: 31 | # varP=something() 32 | # 33 | # pub.publish(varP) 34 | # rate.sleep() 35 | -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Spotmicro - robot dog (http://www.thingiverse.com/thing:3445283) by KDY0523 is licensed under the Creative Commons - Attribution license. 2 | http://creativecommons.org/licenses/by/3.0/ 3 | 4 | -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Back.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Back.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Back_Bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Back_Bracket.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Battery.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Battery.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Chassis_Left_Side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Chassis_Left_Side.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Chassis_Right_Side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Chassis_Right_Side.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Front.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Front.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Front_Bracket.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/Front_Bracket.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_FOOT.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_FOOT.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_HIP.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_HIP.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_LOWER_LEG.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_LOWER_LEG.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_UPPER_LEG.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/LEFT_UPPER_LEG.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/MAINBODY.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/MAINBODY.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_FOOT.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_FOOT.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_HIP.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_HIP.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_LOWER_LEG.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/RIGHT_UPPER_LEG.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/SERVO_WITH_COVER.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/OpenQuadruped/SERVO_WITH_COVER.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/backpart.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/backpart.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/foot.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/frontpart.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/frontpart.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/larm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/larm.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/larm_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/larm_cover.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/lfoot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/lfoot.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/lshoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/lshoulder.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/mainbody.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/mainbody.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rarm.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rarm_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rarm_cover.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rear.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rear.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rear2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rear2.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rearmain.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rearmain.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rfoot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rfoot.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rplidar_main.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rplidar_main.stl -------------------------------------------------------------------------------- /ros-workspace/src/open_quadruped/urdf/stl/rshoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adham-elarabawy/open-quadruped/b0ab4fa599f94460828bf3aa4514240ae917ed38/ros-workspace/src/open_quadruped/urdf/stl/rshoulder.stl -------------------------------------------------------------------------------- /vis-tool/README.md: -------------------------------------------------------------------------------- 1 | ## Visualization Usage 2 | To try the visualization tool out, you'll need to run the [animate.py](https://github.com/adham-elarabawy/OpenQuadruped/blob/master/visualization/animate.py) python file with the proper libraries installed. (matplotlib 3.0.3 supported). 3 | 4 | You can then use keyboard controls: use x, y, z, a, p, r to select (x axis, y axis, z axis, yaw, pitch, roll), and then the up and down buttons to increment the selected position. If you click '1' on your keyboard, it will reset the position. 5 | 6 | Right now, if you try to go to an impossible pose that would result in collisions, the body will do some weird things. If that happens, just click "1" on your keyboard to reset the position. 7 | 8 | *Note: Pitch and Roll are currently not working. I am currently trying to fix that.* 9 | 10 | ### Demo 11 | [![IK Model Visualization](https://img.youtube.com/vi/LBjqJVEXwhM/0.jpg)](https://www.youtube.com/watch?v=LBjqJVEXwhM) 12 | -------------------------------------------------------------------------------- /vis-tool/interface.py: -------------------------------------------------------------------------------- 1 | import math 2 | import sys 3 | 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | 7 | from IK_Engine import Quadruped 8 | 9 | sys.path.append("../control") 10 | 11 | 12 | 13 | # Setting up 3D matplotlib figure 14 | fig = plt.figure() 15 | ax = Axes3D(fig) 16 | ax.set_aspect("equal") 17 | 18 | start_height = 170 19 | WINDOW_SIZE = 500 20 | ax.set_xlim3d(-WINDOW_SIZE / 2, WINDOW_SIZE / 2) 21 | ax.set_ylim3d(-WINDOW_SIZE / 2, WINDOW_SIZE / 2) 22 | ax.set_zlim3d(-start_height, WINDOW_SIZE - start_height) 23 | 24 | ax.set_xlabel('x (mm)') 25 | ax.set_ylabel('y (mm)') 26 | ax.set_zlabel('z (mm)') 27 | 28 | # Setting up Quadruped 29 | robot = Quadruped(ax=ax, origin=(0, 0, 0), height=start_height) 30 | # Going to starting pose 31 | robot.start_position() 32 | # Shifting robot pose in cartesian system x-y-z (body-relative) 33 | robot.shift_body_xyz(30, 0, 0) 34 | # Shifting robot pose in Euler Angles yaw-pitch-roll (body-relative) 35 | robot.shift_body_rotation(math.radians( 36 | 20), math.radians(-10), math.radians(0)) 37 | 38 | 39 | robot.draw_body() 40 | robot.draw_legs() 41 | 42 | plt.show() 43 | -------------------------------------------------------------------------------- /vis-tool/test.py: -------------------------------------------------------------------------------- 1 | Hello all, this is Adham. I am demo-ing the features of git. 2 | --------------------------------------------------------------------------------