├── .catkin_workspace ├── .gitignore ├── .gitmodules ├── .travis.yml ├── CODEOWNERS ├── README.md ├── arduino ├── README.md ├── calibrator │ └── calibrator.ino ├── libraries │ ├── IMU │ │ ├── L3G.cpp │ │ ├── L3G.h │ │ ├── LPS.cpp │ │ ├── LPS.h │ │ ├── LSM303.cpp │ │ └── LSM303.h │ ├── Movement │ │ ├── Movement.cpp │ │ └── Movement.h │ ├── NewPing │ │ ├── NewPing.cpp │ │ └── NewPing.h │ ├── Odometry │ │ ├── Odometry.cpp │ │ └── Odometry.h │ ├── Servo │ │ ├── Servo.cpp │ │ ├── Servo.h │ │ ├── ServoTimers.h │ │ └── library.properties │ ├── lsm303-arduino │ │ ├── .travis.yml │ │ ├── LICENSE.txt │ │ ├── LSM303.cpp │ │ ├── LSM303.h │ │ ├── README.md │ │ ├── examples │ │ │ ├── Calibrate │ │ │ │ └── Calibrate.ino │ │ │ ├── Heading │ │ │ │ └── Heading.ino │ │ │ └── Serial │ │ │ │ └── Serial.ino │ │ ├── keywords.txt │ │ └── library.properties │ └── ros_lib │ │ ├── ArduinoHardware.h │ │ ├── ArduinoTcpHardware.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 │ │ ├── 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 │ │ ├── control_toolbox │ │ └── SetPidGains.h │ │ ├── controller_manager_msgs │ │ ├── ControllerState.h │ │ ├── ControllerStatistics.h │ │ ├── ControllersStatistics.h │ │ ├── HardwareInterfaceResources.h │ │ ├── ListControllerTypes.h │ │ ├── ListControllers.h │ │ ├── LoadController.h │ │ ├── ReloadControllerLibraries.h │ │ ├── SwitchController.h │ │ └── UnloadController.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 │ │ ├── examples │ │ ├── ADC │ │ │ └── ADC.pde │ │ ├── Blink │ │ │ └── Blink.pde │ │ ├── BlinkM │ │ │ ├── BlinkM.pde │ │ │ └── BlinkM_funcs.h │ │ ├── BlinkerWithClass │ │ │ └── BlinkerWithClass.ino │ │ ├── Clapper │ │ │ └── Clapper.pde │ │ ├── Esp8266HelloWorld │ │ │ └── Esp8266HelloWorld.ino │ │ ├── HelloWorld │ │ │ └── HelloWorld.pde │ │ ├── IrRanger │ │ │ └── IrRanger.pde │ │ ├── Logging │ │ │ └── Logging.pde │ │ ├── Odom │ │ │ └── Odom.pde │ │ ├── ServiceClient │ │ │ ├── ServiceClient.pde │ │ │ └── client.py │ │ ├── ServiceServer │ │ │ └── ServiceServer.pde │ │ ├── ServoControl │ │ │ └── ServoControl.pde │ │ ├── TcpBlink │ │ │ └── TcpBlink.ino │ │ ├── TcpHelloWorld │ │ │ └── TcpHelloWorld.ino │ │ ├── Temperature │ │ │ └── Temperature.pde │ │ ├── TimeTF │ │ │ └── TimeTF.pde │ │ ├── Ultrasound │ │ │ └── Ultrasound.pde │ │ ├── button_example │ │ │ └── button_example.pde │ │ └── pubsub │ │ │ └── pubsub.pde │ │ ├── gazebo_msgs │ │ ├── ApplyBodyWrench.h │ │ ├── ApplyJointEffort.h │ │ ├── BodyRequest.h │ │ ├── ContactState.h │ │ ├── ContactsState.h │ │ ├── DeleteLight.h │ │ ├── DeleteModel.h │ │ ├── GetJointProperties.h │ │ ├── GetLightProperties.h │ │ ├── GetLinkProperties.h │ │ ├── GetLinkState.h │ │ ├── GetModelProperties.h │ │ ├── GetModelState.h │ │ ├── GetPhysicsProperties.h │ │ ├── GetWorldProperties.h │ │ ├── JointRequest.h │ │ ├── LinkState.h │ │ ├── LinkStates.h │ │ ├── ModelState.h │ │ ├── ModelStates.h │ │ ├── ODEJointProperties.h │ │ ├── ODEPhysics.h │ │ ├── SetJointProperties.h │ │ ├── SetJointTrajectory.h │ │ ├── SetLightProperties.h │ │ ├── SetLinkProperties.h │ │ ├── SetLinkState.h │ │ ├── SetModelConfiguration.h │ │ ├── SetModelState.h │ │ ├── SetPhysicsProperties.h │ │ ├── SpawnModel.h │ │ └── WorldState.h │ │ ├── geographic_msgs │ │ ├── BoundingBox.h │ │ ├── GeoPath.h │ │ ├── GeoPoint.h │ │ ├── GeoPointStamped.h │ │ ├── GeoPose.h │ │ ├── GeoPoseStamped.h │ │ ├── GeographicMap.h │ │ ├── GeographicMapChanges.h │ │ ├── GetGeoPath.h │ │ ├── GetGeographicMap.h │ │ ├── GetRoutePlan.h │ │ ├── KeyValue.h │ │ ├── MapFeature.h │ │ ├── RouteNetwork.h │ │ ├── RoutePath.h │ │ ├── RouteSegment.h │ │ ├── UpdateGeographicMap.h │ │ └── WayPoint.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 │ │ ├── grid_map_msgs │ │ ├── GetGridMap.h │ │ ├── GetGridMapInfo.h │ │ ├── GridMap.h │ │ ├── GridMapInfo.h │ │ └── ProcessFile.h │ │ ├── hector_gazebo_plugins │ │ └── SetBias.h │ │ ├── laser_assembler │ │ ├── AssembleScans.h │ │ └── AssembleScans2.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 │ │ ├── octomap_msgs │ │ ├── BoundingBoxQuery.h │ │ ├── GetOctomap.h │ │ ├── Octomap.h │ │ └── OctomapWithPose.h │ │ ├── pcl_msgs │ │ ├── ModelCoefficients.h │ │ ├── PointIndices.h │ │ ├── PolygonMesh.h │ │ └── Vertices.h │ │ ├── polled_camera │ │ └── GetPolledImage.h │ │ ├── robot_localization │ │ ├── GetState.h │ │ ├── SetDatum.h │ │ └── SetPose.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 │ │ ├── tests │ │ ├── array_test │ │ │ └── array_test.pde │ │ ├── float64_test │ │ │ └── float64_test.pde │ │ └── time_test │ │ │ └── time_test.pde │ │ ├── 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 │ │ ├── theora_image_transport │ │ └── Packet.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 │ │ ├── uuid_msgs │ │ └── UniqueID.h │ │ ├── variant_msgs │ │ ├── Test.h │ │ ├── Variant.h │ │ ├── VariantHeader.h │ │ └── VariantType.h │ │ ├── visualization_msgs │ │ ├── ImageMarker.h │ │ ├── InteractiveMarker.h │ │ ├── InteractiveMarkerControl.h │ │ ├── InteractiveMarkerFeedback.h │ │ ├── InteractiveMarkerInit.h │ │ ├── InteractiveMarkerPose.h │ │ ├── InteractiveMarkerUpdate.h │ │ ├── Marker.h │ │ ├── MarkerArray.h │ │ └── MenuEntry.h │ │ └── wiimote │ │ ├── IrSourceInfo.h │ │ ├── State.h │ │ └── TimedSwitch.h ├── readmeImages │ ├── ArduinoDebugDataInput.png │ ├── ArduinoDebugDataOutput.png │ ├── ArduinoDebugFingersOpen.png │ ├── ArduinoDebugHighCenter.png │ ├── ArduinoDebugWristDown.png │ ├── ArduinoIDEBoardType.png │ ├── ArduinoIDELineEndingBaudRate.png │ ├── ArduinoIDEOpenSketch.png │ ├── ArduinoIDEOpenSketch2.png │ ├── ArduinoIDESerialMonitor.png │ ├── ArduinoIDESerialPort.png │ ├── ArduinoIDESketchbookLocation.png │ ├── ArduinoIDEUploadSketch.png │ └── ArduinoIDEUploadSuccess.png └── swarmie_control │ └── swarmie_control.ino ├── camera_info └── head_camera.yaml ├── cleanup.sh ├── launch ├── .launch └── swarmie.launch ├── license.txt ├── misc ├── calibrate.py ├── deploy.sh ├── gen_rover_models.sh ├── get_arduino_port.sh ├── load_swarmie_control_sketch.sh ├── logs │ └── empty.txt ├── nohup.out ├── rosbridge │ ├── eventemitter2.min.js │ ├── jquery-1.9.1.min.js │ ├── robot_interface.html │ ├── roslib.min.js │ ├── simple-node.js │ ├── swarmie-web.js │ └── swarmie.js ├── rover_launch_local.sh ├── rover_onboard_node_launch.sh ├── run_calibration.sh ├── start_bridge.sh ├── start_robots.sh └── stop_robots.sh ├── readmeImages ├── InstallGraphics.png ├── activeRovers.png ├── buildSim.png ├── guiFirstScreen.png ├── roverSensorOutputs.png ├── sensorDisplayTab.png ├── simControlTab.png └── taskStatusTab.png ├── recruitment-demo.md ├── run.sh ├── run_headless_sim.sh ├── run_tests.sh ├── simulation ├── launch │ └── swarmathon.launch ├── models │ ├── achilles │ │ ├── model.config │ │ └── model.sdf │ ├── aeneas │ │ ├── model.config │ │ └── model.sdf │ ├── ajax │ │ ├── model.config │ │ └── model.sdf │ ├── at0 │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── at0.material │ │ │ └── textures │ │ │ │ └── atag-0.png │ │ ├── model.config │ │ └── model.sdf │ ├── at1 │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── at1.material │ │ │ └── textures │ │ │ │ └── atag-1.png │ │ ├── model.config │ │ └── model.sdf │ ├── at1_barrier_final_round │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── at1.material │ │ │ └── textures │ │ │ │ └── atag-1.png │ │ ├── model.config │ │ └── model.sdf │ ├── at1_barrier_prelim_round │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── at1.material │ │ │ └── textures │ │ │ │ └── atag-1.png │ │ ├── model.config │ │ └── model.sdf │ ├── barrier_final_round │ │ ├── model.config │ │ └── model.sdf │ ├── barrier_prelim_round │ │ ├── model.config │ │ └── model.sdf │ ├── boulder1 │ │ ├── boulder1.dae │ │ ├── boulder1.material │ │ ├── boulder1.png │ │ ├── model.config │ │ └── model.sdf │ ├── boulder2 │ │ ├── boulder2.dae │ │ ├── boulder2.material │ │ ├── boulder2.png │ │ ├── model.config │ │ └── model.sdf │ ├── box │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── box.material │ │ │ └── textures │ │ │ │ └── boxTexture.jpg │ │ ├── model.config │ │ └── model.sdf │ ├── carpark_ground_plane │ │ ├── model.config │ │ └── model.sdf │ ├── cinder_block │ │ ├── model.config │ │ └── model.sdf │ ├── collection_disk │ │ ├── materials │ │ │ ├── scripts │ │ │ │ └── collection_disk.material │ │ │ └── textures │ │ │ │ ├── collection_disk.pdf │ │ │ │ └── collection_disk.png │ │ ├── model.config │ │ └── model.sdf │ ├── concrete_ground_plane │ │ ├── model.config │ │ └── model.sdf │ ├── diomedes │ │ ├── model.config │ │ └── model.sdf │ ├── hector │ │ ├── model.config │ │ └── model.sdf │ ├── lunar_lander │ │ ├── lunar_lander.stl │ │ ├── lunar_lander_base.stl │ │ ├── model.config │ │ └── model.sdf │ ├── mars_ground_plane │ │ ├── model.config │ │ └── model.sdf │ ├── media │ │ └── materials │ │ │ ├── scripts │ │ │ ├── brick.material │ │ │ ├── carpark_ground_plane.material │ │ │ ├── collection_disk.material │ │ │ ├── concrete_ground_plane.material │ │ │ ├── mars_ground_plane.material │ │ │ └── white.material │ │ │ └── textures │ │ │ ├── GravelImage.png │ │ │ ├── GravelImage_old.png │ │ │ ├── asphalt.jpg │ │ │ ├── carpark.jpg │ │ │ ├── carpark.png │ │ │ ├── carpark2.jpg │ │ │ ├── collection_disk.png │ │ │ ├── concrete.jpg │ │ │ ├── concrete2.jpg │ │ │ ├── concreteTile.jpg │ │ │ ├── ksc_carpark2.jpg │ │ │ ├── ksc_carpark3.jpg │ │ │ └── white.png │ ├── paris │ │ ├── model.config │ │ └── model.sdf │ ├── red_sky_filter │ │ ├── model.config │ │ └── model.sdf │ ├── rock_barrier │ │ ├── model.config │ │ ├── model.sdf │ │ ├── rock_barrier.material │ │ ├── rock_barrier.png │ │ └── rock_barrier.stl │ ├── rock_bridge │ │ ├── model.config │ │ ├── model.sdf │ │ ├── rock_bridge.dae │ │ ├── rock_bridge.material │ │ └── rock_bridge.png │ ├── swarmie │ │ ├── README.md │ │ ├── meshes │ │ │ ├── Body.stl │ │ │ ├── CoverPlate.stl │ │ │ ├── CowCatcherFront.stl │ │ │ ├── CowCatcherRear.stl │ │ │ ├── GripperFingerLeft.stl │ │ │ ├── GripperFingerRight.stl │ │ │ ├── GripperWrist.stl │ │ │ ├── TireWheelLeft.stl │ │ │ └── TireWheelRight.stl │ │ ├── model.config.erb │ │ └── model.sdf.erb │ ├── thor │ │ ├── model.config │ │ └── model.sdf │ └── zeus │ │ ├── model.config │ │ └── model.sdf └── worlds │ ├── clustered_targets_example.world │ ├── mars_diorama.world │ ├── powerlaw_targets_example.world │ ├── simple_obstacle_example.world │ ├── swarmathon.world │ └── uniform_targets_example.world └── src ├── .idea ├── .name ├── encodings.xml ├── misc.xml ├── modules.xml ├── src.iml └── workspace.xml ├── CMakeLists.txt ├── abridge ├── CMakeLists.txt ├── include │ └── usbSerial.h ├── package.xml └── src │ ├── abridge.cpp │ └── usbSerial.cpp ├── behaviours ├── CMakeLists.txt ├── package.xml └── src │ ├── Controller.h │ ├── DriveController.cpp │ ├── DriveController.h │ ├── DropOffController.cpp │ ├── DropOffController.h │ ├── LogicController.cpp │ ├── LogicController.h │ ├── ManualWaypointController.cpp │ ├── ManualWaypointController.h │ ├── ObstacleController.cpp │ ├── ObstacleController.h │ ├── PID.cpp │ ├── PID.h │ ├── PickUpController.cpp │ ├── PickUpController.h │ ├── Point.h │ ├── PositionPublisher.cpp │ ├── PositionPublisher.hpp │ ├── ROSAdapter.cpp │ ├── RangeController.cpp │ ├── RangeController.h │ ├── Result.h │ ├── SearchController.cpp │ ├── SearchController.h │ ├── Tag.cpp │ └── Tag.h ├── diagnostics ├── CMakeLists.txt ├── package.xml └── src │ ├── Diagnostics.cpp │ ├── Diagnostics.h │ ├── WirelessDiags.cpp │ ├── WirelessDiags.h │ └── driver.cpp ├── gazebo_plugins ├── CMakeLists.txt ├── package.xml └── src │ ├── GripperPlugin │ ├── GripperManager.cpp │ ├── GripperManager.h │ ├── GripperPlugin.cpp │ ├── GripperPlugin.h │ ├── PIDController.cpp │ ├── PIDController.h │ └── README.md │ ├── ScorePlugin │ ├── README.md │ ├── ScorePlugin.cpp │ └── ScorePlugin.h │ └── SetupWorld.cpp ├── rqt_rover_gui ├── CMakeLists.txt ├── launch │ └── rover_gui.launch ├── package.xml ├── plugin.xml ├── resources │ ├── images.jpeg │ ├── resources.qrc │ ├── rover.png │ └── swarmathon-gui-icon.png ├── scripts │ └── rqt_rover_gui ├── setup.py └── src │ ├── BWTabWidget.cpp │ ├── BWTabWidget.h │ ├── CameraFrame.cpp │ ├── CameraFrame.h │ ├── GPSFrame.cpp │ ├── GPSFrame.h │ ├── GazeboSimManager.cpp │ ├── GazeboSimManager.h │ ├── IMUFrame.cpp │ ├── IMUFrame.h │ ├── JoystickGripperInterface.cpp │ ├── JoystickGripperInterface.h │ ├── MapData.cpp │ ├── MapData.h │ ├── MapFrame.cpp │ ├── MapFrame.h │ ├── USFrame.cpp │ ├── USFrame.h │ ├── Version.cpp.in │ ├── Version.h │ ├── rover_gui_plugin.cpp │ ├── rover_gui_plugin.h │ └── rover_gui_plugin.ui ├── sbridge ├── CMakeLists.txt ├── package.xml └── src │ ├── main.cpp │ ├── sbridge.cpp │ └── sbridge.h └── swarmie_msgs ├── CMakeLists.txt ├── msg ├── Recruitment.msg ├── Skid.msg └── Waypoint.msg └── package.xml /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | \#*# 3 | build 4 | devel 5 | **/CMakeLists.txt.user* 6 | *.swp 7 | logs 8 | .catkin_tools 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/ublox"] 2 | path = src/ublox 3 | url = https://github.com/KumarRobotics/ublox.git 4 | [submodule "src/apriltags_ros"] 5 | path = src/apriltags_ros 6 | url = https://github.com/BCLab-UNM/apriltags_ros.git 7 | branch = kinetic-master 8 | [submodule "src/usb_cam"] 9 | path = src/usb_cam 10 | url = https://github.com/bosch-ros-pkg/usb_cam.git 11 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | dist: xenial 4 | sudo: required 5 | 6 | env: 7 | global: 8 | - ros_distro: kinetic 9 | 10 | before_install: 11 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 12 | - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 13 | - sudo apt update -qq 14 | - sudo apt install -y ros-$ros_distro-desktop-full 15 | - sudo rosdep init 16 | - rosdep update 17 | - sudo apt install -y libusb-dev python-rosinstall python-catkin-tools ros-$ros_distro-robot-localization ros-$ros_distro-usb-cam ros-$ros_distro-hector-gazebo-plugins ros-$ros_distro-joystick-drivers 18 | - source /opt/ros/$ros_distro/setup.bash 19 | 20 | script: 21 | - source /opt/ros/$ros_distro/setup.bash 22 | - catkin build 23 | - ./run_tests.sh 24 | # - roscore & 25 | # - catkin run_tests 26 | # - source devel/setup.bash 27 | # - devel/lib/behaviours/behaviour_test 28 | 29 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @gmfricke 2 | -------------------------------------------------------------------------------- /arduino/libraries/Movement/Movement.h: -------------------------------------------------------------------------------- 1 | #ifndef Movement_h 2 | #define Movement_h 3 | 4 | #include "Arduino.h" 5 | 6 | class Movement 7 | { 8 | public: 9 | //Constructors 10 | Movement(byte rightSpeedPin, byte rightDirectionA, byte rightDirectionB, byte leftSpeedPin, byte leftDirectionA, byte leftDirectionB); 11 | 12 | //Functions 13 | void forward(byte leftSpeed, byte rightSpeed); 14 | void backward(byte leftSpeed, byte rightSpeed); 15 | void rotateRight(byte leftSpeed, byte rightSpeed); 16 | void rotateLeft(byte leftSpeed, byte rightSpeed); 17 | void stop(); 18 | 19 | private: 20 | byte _rightSpeedPin, _rightDirectionA, _rightDirectionB, _leftSpeedPin, _leftDirectionA, _leftDirectionB; 21 | }; 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /arduino/libraries/Odometry/Odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef Odometry_h 2 | #define Odometry_h 3 | 4 | #include "Arduino.h" 5 | 6 | class Odometry { 7 | public: 8 | //Constructors 9 | Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin, float wheelBase, float wheelDiameter, int cpr); 10 | 11 | //Functions 12 | void update(); 13 | 14 | //Variables 15 | float x, y, theta; 16 | float vx, vy, vtheta; 17 | long clock; 18 | 19 | private: 20 | //Variables 21 | float _wheelBase, _wheelDiameter; 22 | int _cpr; 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /arduino/libraries/Servo/library.properties: -------------------------------------------------------------------------------- 1 | name=Servo 2 | version=1.1.2 3 | author=Michael Margolis, Arduino 4 | maintainer=Arduino 5 | sentence=Allows Arduino/Genuino boards to control a variety of servo motors. 6 | paragraph=This library can control a great number of servos.
It makes careful use of timers: the library can control 12 servos using only 1 timer.
On the Arduino Due you can control up to 60 servos.
7 | category=Device Control 8 | url=http://www.arduino.cc/en/Reference/Servo 9 | architectures=avr -------------------------------------------------------------------------------- /arduino/libraries/lsm303-arduino/.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | 3 | cache: 4 | directories: 5 | - "~/.platformio" 6 | 7 | install: 8 | - pip install -U platformio 9 | 10 | env: 11 | - BOARD=uno 12 | - BOARD=leonardo 13 | - BOARD=micro 14 | - BOARD=megaatmega2560 15 | - BOARD=due 16 | - BOARD=yun 17 | - BOARD=genuino101 18 | - BOARD=zero 19 | 20 | script: 21 | - set -eo pipefail; 22 | for e in examples/*; do 23 | platformio ci --board=$BOARD --lib=. $e/*; 24 | done 25 | -------------------------------------------------------------------------------- /arduino/libraries/lsm303-arduino/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013 Pololu Corporation. For more information, see 2 | 3 | http://www.pololu.com/ 4 | http://forum.pololu.com/ 5 | 6 | Permission is hereby granted, free of charge, to any person 7 | obtaining a copy of this software and associated documentation 8 | files (the "Software"), to deal in the Software without 9 | restriction, including without limitation the rights to use, 10 | copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the 12 | Software is furnished to do so, subject to the following 13 | conditions: 14 | 15 | The above copyright notice and this permission notice shall be 16 | included in all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 19 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 20 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 21 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 22 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 23 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 24 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 25 | OTHER DEALINGS IN THE SOFTWARE. 26 | -------------------------------------------------------------------------------- /arduino/libraries/lsm303-arduino/examples/Calibrate/Calibrate.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | LSM303 compass; 5 | LSM303::vector running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768}; 6 | 7 | char report[80]; 8 | 9 | void setup() { 10 | Serial.begin(9600); 11 | Wire.begin(); 12 | compass.init(); 13 | compass.enableDefault(); 14 | } 15 | 16 | void loop() { 17 | compass.read(); 18 | 19 | running_min.x = min(running_min.x, compass.m.x); 20 | running_min.y = min(running_min.y, compass.m.y); 21 | running_min.z = min(running_min.z, compass.m.z); 22 | 23 | running_max.x = max(running_max.x, compass.m.x); 24 | running_max.y = max(running_max.y, compass.m.y); 25 | running_max.z = max(running_max.z, compass.m.z); 26 | 27 | snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d} max: {%+6d, %+6d, %+6d}", 28 | running_min.x, running_min.y, running_min.z, 29 | running_max.x, running_max.y, running_max.z); 30 | Serial.println(report); 31 | 32 | delay(100); 33 | } -------------------------------------------------------------------------------- /arduino/libraries/lsm303-arduino/examples/Heading/Heading.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | LSM303 compass; 5 | 6 | void setup() { 7 | Serial.begin(9600); 8 | Wire.begin(); 9 | compass.init(); 10 | compass.enableDefault(); 11 | 12 | /* 13 | Calibration values; the default values of +/-32767 for each axis 14 | lead to an assumed magnetometer bias of 0. Use the Calibrate example 15 | program to determine appropriate values for your particular unit. 16 | */ 17 | compass.m_min = (LSM303::vector){-32767, -32767, -32767}; 18 | compass.m_max = (LSM303::vector){+32767, +32767, +32767}; 19 | } 20 | 21 | void loop() { 22 | compass.read(); 23 | 24 | /* 25 | When given no arguments, the heading() function returns the angular 26 | difference in the horizontal plane between a default vector and 27 | north, in degrees. 28 | 29 | The default vector is chosen by the library to point along the 30 | surface of the PCB, in the direction of the top of the text on the 31 | silkscreen. This is the +X axis on the Pololu LSM303D carrier and 32 | the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH 33 | carriers. 34 | 35 | To use a different vector as a reference, use the version of heading() 36 | that takes a vector argument; for example, use 37 | 38 | compass.heading((LSM303::vector){0, 0, 1}); 39 | 40 | to use the +Z axis as a reference. 41 | */ 42 | float heading = compass.heading(); 43 | 44 | Serial.println(heading); 45 | delay(100); 46 | } -------------------------------------------------------------------------------- /arduino/libraries/lsm303-arduino/library.properties: -------------------------------------------------------------------------------- 1 | name=LSM303 2 | version=3.0.1 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Arduino library for Pololu LSM303 boards 6 | paragraph=This is a library for an Arduino-compatible controller that interfaces with LSM303D, LSM303DLHC, LSM303DLM, and LSM303DLH 3D compass and accelerometer ICs on Pololu boards. 7 | category=Sensors 8 | url=https://github.com/pololu/lsm303-arduino 9 | architectures=* -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/control_msgs/GripperCommand.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_GripperCommand_h 2 | #define _ROS_control_msgs_GripperCommand_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class GripperCommand : public ros::Msg 13 | { 14 | public: 15 | typedef float _position_type; 16 | _position_type position; 17 | typedef float _max_effort_type; 18 | _max_effort_type max_effort; 19 | 20 | GripperCommand(): 21 | position(0), 22 | max_effort(0) 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += serializeAvrFloat64(outbuffer + offset, this->position); 30 | offset += serializeAvrFloat64(outbuffer + offset, this->max_effort); 31 | return offset; 32 | } 33 | 34 | virtual int deserialize(unsigned char *inbuffer) 35 | { 36 | int offset = 0; 37 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->position)); 38 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->max_effort)); 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "control_msgs/GripperCommand"; }; 43 | const char * getMD5(){ return "680acaff79486f017132a7f198d40f08"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/control_msgs/PointHeadFeedback.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_control_msgs_PointHeadFeedback_h 2 | #define _ROS_control_msgs_PointHeadFeedback_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace control_msgs 10 | { 11 | 12 | class PointHeadFeedback : public ros::Msg 13 | { 14 | public: 15 | typedef float _pointing_angle_error_type; 16 | _pointing_angle_error_type pointing_angle_error; 17 | 18 | PointHeadFeedback(): 19 | pointing_angle_error(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | offset += serializeAvrFloat64(outbuffer + offset, this->pointing_angle_error); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->pointing_angle_error)); 34 | return offset; 35 | } 36 | 37 | const char * getType(){ return "control_msgs/PointHeadFeedback"; }; 38 | const char * getMD5(){ return "cce80d27fd763682da8805a73316cab4"; }; 39 | 40 | }; 41 | 42 | } 43 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/ADC/ADC.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial ADC Example 3 | * 4 | * This is a poor man's Oscilloscope. It does not have the sampling 5 | * rate or accuracy of a commerical scope, but it is great to get 6 | * an analog value into ROS in a pinch. 7 | */ 8 | 9 | #if (ARDUINO >= 100) 10 | #include 11 | #else 12 | #include 13 | #endif 14 | #include 15 | #include 16 | 17 | ros::NodeHandle nh; 18 | 19 | rosserial_arduino::Adc adc_msg; 20 | ros::Publisher p("adc", &adc_msg); 21 | 22 | void setup() 23 | { 24 | pinMode(13, OUTPUT); 25 | nh.initNode(); 26 | 27 | nh.advertise(p); 28 | } 29 | 30 | //We average the analog reading to elminate some of the noise 31 | int averageAnalog(int pin){ 32 | int v=0; 33 | for(int i=0; i<4; i++) v+= analogRead(pin); 34 | return v/4; 35 | } 36 | 37 | long adc_timer; 38 | 39 | void loop() 40 | { 41 | adc_msg.adc0 = averageAnalog(0); 42 | adc_msg.adc1 = averageAnalog(1); 43 | adc_msg.adc2 = averageAnalog(2); 44 | adc_msg.adc3 = averageAnalog(3); 45 | adc_msg.adc4 = averageAnalog(4); 46 | adc_msg.adc5 = averageAnalog(5); 47 | 48 | p.publish(&adc_msg); 49 | 50 | nh.spinOnce(); 51 | } 52 | 53 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/Blink/Blink.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example 3 | * Blinks an LED on callback 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | void messageCb( const std_msgs::Empty& toggle_msg){ 12 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 13 | } 14 | 15 | ros::Subscriber sub("toggle_led", &messageCb ); 16 | 17 | void setup() 18 | { 19 | pinMode(13, OUTPUT); 20 | nh.initNode(); 21 | nh.subscribe(sub); 22 | } 23 | 24 | void loop() 25 | { 26 | nh.spinOnce(); 27 | delay(1); 28 | } 29 | 30 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/HelloWorld/HelloWorld.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Publisher Example 3 | * Prints "hello world!" 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::String str_msg; 12 | ros::Publisher chatter("chatter", &str_msg); 13 | 14 | char hello[13] = "hello world!"; 15 | 16 | void setup() 17 | { 18 | nh.initNode(); 19 | nh.advertise(chatter); 20 | } 21 | 22 | void loop() 23 | { 24 | str_msg.data = hello; 25 | chatter.publish( &str_msg ); 26 | nh.spinOnce(); 27 | delay(1000); 28 | } 29 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/Logging/Logging.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | std_msgs::String str_msg; 14 | ros::Publisher chatter("chatter", &str_msg); 15 | 16 | char hello[13] = "hello world!"; 17 | 18 | 19 | char debug[]= "debug statements"; 20 | char info[] = "infos"; 21 | char warn[] = "warnings"; 22 | char error[] = "errors"; 23 | char fatal[] = "fatalities"; 24 | 25 | void setup() 26 | { 27 | pinMode(13, OUTPUT); 28 | nh.initNode(); 29 | nh.advertise(chatter); 30 | } 31 | 32 | void loop() 33 | { 34 | str_msg.data = hello; 35 | chatter.publish( &str_msg ); 36 | 37 | nh.logdebug(debug); 38 | nh.loginfo(info); 39 | nh.logwarn(warn); 40 | nh.logerror(error); 41 | nh.logfatal(fatal); 42 | 43 | nh.spinOnce(); 44 | delay(500); 45 | } 46 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/Odom/Odom.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Planar Odometry Example 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | double x = 1.0; 16 | double y = 0.0; 17 | double theta = 1.57; 18 | 19 | char base_link[] = "/base_link"; 20 | char odom[] = "/odom"; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | broadcaster.init(nh); 26 | } 27 | 28 | void loop() 29 | { 30 | // drive in a circle 31 | double dx = 0.2; 32 | double dtheta = 0.18; 33 | x += cos(theta)*dx*0.1; 34 | y += sin(theta)*dx*0.1; 35 | theta += dtheta*0.1; 36 | if(theta > 3.14) 37 | theta=-3.14; 38 | 39 | // tf odom->base_link 40 | t.header.frame_id = odom; 41 | t.child_frame_id = base_link; 42 | 43 | t.transform.translation.x = x; 44 | t.transform.translation.y = y; 45 | 46 | t.transform.rotation = tf::createQuaternionFromYaw(theta); 47 | t.header.stamp = nh.now(); 48 | 49 | broadcaster.sendTransform(t); 50 | nh.spinOnce(); 51 | 52 | delay(10); 53 | } 54 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/ServiceClient/ServiceClient.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Client 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | ros::ServiceClient client("test_srv"); 13 | 14 | std_msgs::String str_msg; 15 | ros::Publisher chatter("chatter", &str_msg); 16 | 17 | char hello[13] = "hello world!"; 18 | 19 | void setup() 20 | { 21 | nh.initNode(); 22 | nh.serviceClient(client); 23 | nh.advertise(chatter); 24 | while(!nh.connected()) nh.spinOnce(); 25 | nh.loginfo("Startup complete"); 26 | } 27 | 28 | void loop() 29 | { 30 | Test::Request req; 31 | Test::Response res; 32 | req.input = hello; 33 | client.call(req, res); 34 | str_msg.data = res.output; 35 | chatter.publish( &str_msg ); 36 | nh.spinOnce(); 37 | delay(100); 38 | } 39 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/ServiceClient/client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sample code to use with ServiceClient.pde 5 | """ 6 | 7 | import roslib; roslib.load_manifest("rosserial_arduino") 8 | import rospy 9 | 10 | from rosserial_arduino.srv import * 11 | 12 | def callback(req): 13 | print "The arduino is calling! Please send it a message:" 14 | t = TestResponse() 15 | t.output = raw_input() 16 | return t 17 | 18 | rospy.init_node("service_client_test") 19 | rospy.Service("test_srv", Test, callback) 20 | rospy.spin() 21 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/ServiceServer/ServiceServer.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Service Server 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ros::NodeHandle nh; 10 | using rosserial_arduino::Test; 11 | 12 | int i; 13 | void callback(const Test::Request & req, Test::Response & res){ 14 | if((i++)%2) 15 | res.output = "hello"; 16 | else 17 | res.output = "world"; 18 | } 19 | 20 | ros::ServiceServer server("test_srv",&callback); 21 | 22 | std_msgs::String str_msg; 23 | ros::Publisher chatter("chatter", &str_msg); 24 | 25 | char hello[13] = "hello world!"; 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertiseService(server); 31 | nh.advertise(chatter); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(10); 40 | } 41 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/ServoControl/ServoControl.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Servo Control Example 3 | * 4 | * This sketch demonstrates the control of hobby R/C servos 5 | * using ROS and the arduiono 6 | * 7 | * For the full tutorial write up, visit 8 | * www.ros.org/wiki/rosserial_arduino_demos 9 | * 10 | * For more information on the Arduino Servo Library 11 | * Checkout : 12 | * http://www.arduino.cc/en/Reference/Servo 13 | */ 14 | 15 | #if (ARDUINO >= 100) 16 | #include 17 | #else 18 | #include 19 | #endif 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | ros::NodeHandle nh; 26 | 27 | Servo servo; 28 | 29 | void servo_cb( const std_msgs::UInt16& cmd_msg){ 30 | servo.write(cmd_msg.data); //set servo angle, should be from 0-180 31 | digitalWrite(13, HIGH-digitalRead(13)); //toggle led 32 | } 33 | 34 | 35 | ros::Subscriber sub("servo", servo_cb); 36 | 37 | void setup(){ 38 | pinMode(13, OUTPUT); 39 | 40 | nh.initNode(); 41 | nh.subscribe(sub); 42 | 43 | servo.attach(9); //attach it to pin 9 44 | } 45 | 46 | void loop(){ 47 | nh.spinOnce(); 48 | delay(1); 49 | } 50 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/TcpBlink/TcpBlink.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Subscriber Example using TCP on Arduino Shield (Wiznet W5100 based) 3 | * Blinks an LED on callback 4 | */ 5 | #include 6 | #include 7 | 8 | #define ROSSERIAL_ARDUINO_TCP 9 | 10 | #include 11 | #include 12 | 13 | ros::NodeHandle nh; 14 | 15 | // Shield settings 16 | byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; 17 | IPAddress ip(192, 168, 0, 177); 18 | 19 | // Server settings 20 | IPAddress server(192, 168, 0, 11); 21 | uint16_t serverPort = 11411; 22 | 23 | const uint8_t ledPin = 6; // 13 already used for SPI connection with the shield 24 | 25 | void messageCb( const std_msgs::Empty&){ 26 | digitalWrite(ledPin, HIGH-digitalRead(ledPin)); // blink the led 27 | } 28 | 29 | ros::Subscriber sub("toggle_led", &messageCb ); 30 | 31 | void setup() 32 | { 33 | Ethernet.begin(mac, ip); 34 | // give the Ethernet shield a second to initialize: 35 | delay(1000); 36 | pinMode(ledPin, OUTPUT); 37 | nh.getHardware()->setConnection(server, serverPort); 38 | nh.initNode(); 39 | nh.subscribe(sub); 40 | } 41 | 42 | void loop() 43 | { 44 | nh.spinOnce(); 45 | delay(1); 46 | } 47 | 48 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/TimeTF/TimeTF.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Time and TF Example 3 | * Publishes a transform at current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | geometry_msgs::TransformStamped t; 13 | tf::TransformBroadcaster broadcaster; 14 | 15 | char base_link[] = "/base_link"; 16 | char odom[] = "/odom"; 17 | 18 | void setup() 19 | { 20 | nh.initNode(); 21 | broadcaster.init(nh); 22 | } 23 | 24 | void loop() 25 | { 26 | t.header.frame_id = odom; 27 | t.child_frame_id = base_link; 28 | t.transform.translation.x = 1.0; 29 | t.transform.rotation.x = 0.0; 30 | t.transform.rotation.y = 0.0; 31 | t.transform.rotation.z = 0.0; 32 | t.transform.rotation.w = 1.0; 33 | t.header.stamp = nh.now(); 34 | broadcaster.sendTransform(t); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/Ultrasound/Ultrasound.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Ultrasound Example 3 | * 4 | * This example is for the Maxbotix Ultrasound rangers. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | ros::NodeHandle nh; 12 | 13 | sensor_msgs::Range range_msg; 14 | ros::Publisher pub_range( "/ultrasound", &range_msg); 15 | 16 | const int adc_pin = 0; 17 | 18 | char frameid[] = "/ultrasound"; 19 | 20 | float getRange_Ultrasound(int pin_num){ 21 | int val = 0; 22 | for(int i=0; i<4; i++) val += analogRead(pin_num); 23 | float range = val; 24 | return range /322.519685; // (0.0124023437 /4) ; //cvt to meters 25 | } 26 | 27 | void setup() 28 | { 29 | nh.initNode(); 30 | nh.advertise(pub_range); 31 | 32 | 33 | range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; 34 | range_msg.header.frame_id = frameid; 35 | range_msg.field_of_view = 0.1; // fake 36 | range_msg.min_range = 0.0; 37 | range_msg.max_range = 6.47; 38 | 39 | pinMode(8,OUTPUT); 40 | digitalWrite(8, LOW); 41 | } 42 | 43 | 44 | long range_time; 45 | 46 | void loop() 47 | { 48 | 49 | //publish the adc value every 50 milliseconds 50 | //since it takes that long for the sensor to stablize 51 | if ( millis() >= range_time ){ 52 | int r =0; 53 | 54 | range_msg.range = getRange_Ultrasound(5); 55 | range_msg.header.stamp = nh.now(); 56 | pub_range.publish(&range_msg); 57 | range_time = millis() + 50; 58 | } 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/button_example/button_example.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * Button Example for Rosserial 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | 9 | ros::NodeHandle nh; 10 | 11 | std_msgs::Bool pushed_msg; 12 | ros::Publisher pub_button("pushed", &pushed_msg); 13 | 14 | const int button_pin = 7; 15 | const int led_pin = 13; 16 | 17 | bool last_reading; 18 | long last_debounce_time=0; 19 | long debounce_delay=50; 20 | bool published = true; 21 | 22 | void setup() 23 | { 24 | nh.initNode(); 25 | nh.advertise(pub_button); 26 | 27 | //initialize an LED output pin 28 | //and a input pin for our push button 29 | pinMode(led_pin, OUTPUT); 30 | pinMode(button_pin, INPUT); 31 | 32 | //Enable the pullup resistor on the button 33 | digitalWrite(button_pin, HIGH); 34 | 35 | //The button is a normally button 36 | last_reading = ! digitalRead(button_pin); 37 | 38 | } 39 | 40 | void loop() 41 | { 42 | 43 | bool reading = ! digitalRead(button_pin); 44 | 45 | if (last_reading!= reading){ 46 | last_debounce_time = millis(); 47 | published = false; 48 | } 49 | 50 | //if the button value has not changed for the debounce delay, we know its stable 51 | if ( !published && (millis() - last_debounce_time) > debounce_delay) { 52 | digitalWrite(led_pin, reading); 53 | pushed_msg.data = reading; 54 | pub_button.publish(&pushed_msg); 55 | published = true; 56 | } 57 | 58 | last_reading = reading; 59 | 60 | nh.spinOnce(); 61 | } 62 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/examples/pubsub/pubsub.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial PubSub Example 3 | * Prints "hello world!" and toggles led 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | ros::NodeHandle nh; 11 | 12 | 13 | void messageCb( const std_msgs::Empty& toggle_msg){ 14 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 15 | } 16 | 17 | ros::Subscriber sub("toggle_led", messageCb ); 18 | 19 | 20 | 21 | std_msgs::String str_msg; 22 | ros::Publisher chatter("chatter", &str_msg); 23 | 24 | char hello[13] = "hello world!"; 25 | 26 | void setup() 27 | { 28 | pinMode(13, OUTPUT); 29 | nh.initNode(); 30 | nh.advertise(chatter); 31 | nh.subscribe(sub); 32 | } 33 | 34 | void loop() 35 | { 36 | str_msg.data = hello; 37 | chatter.publish( &str_msg ); 38 | nh.spinOnce(); 39 | delay(500); 40 | } 41 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geographic_msgs/BoundingBox.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geographic_msgs_BoundingBox_h 2 | #define _ROS_geographic_msgs_BoundingBox_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geographic_msgs/GeoPoint.h" 9 | 10 | namespace geographic_msgs 11 | { 12 | 13 | class BoundingBox : public ros::Msg 14 | { 15 | public: 16 | typedef geographic_msgs::GeoPoint _min_pt_type; 17 | _min_pt_type min_pt; 18 | typedef geographic_msgs::GeoPoint _max_pt_type; 19 | _max_pt_type max_pt; 20 | 21 | BoundingBox(): 22 | min_pt(), 23 | max_pt() 24 | { 25 | } 26 | 27 | virtual int serialize(unsigned char *outbuffer) const 28 | { 29 | int offset = 0; 30 | offset += this->min_pt.serialize(outbuffer + offset); 31 | offset += this->max_pt.serialize(outbuffer + offset); 32 | return offset; 33 | } 34 | 35 | virtual int deserialize(unsigned char *inbuffer) 36 | { 37 | int offset = 0; 38 | offset += this->min_pt.deserialize(inbuffer + offset); 39 | offset += this->max_pt.deserialize(inbuffer + offset); 40 | return offset; 41 | } 42 | 43 | const char * getType(){ return "geographic_msgs/BoundingBox"; }; 44 | const char * getMD5(){ return "f62e8b5e390a3ac7603250d46e8f8446"; }; 45 | 46 | }; 47 | 48 | } 49 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geographic_msgs/GeoPoint.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geographic_msgs_GeoPoint_h 2 | #define _ROS_geographic_msgs_GeoPoint_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace geographic_msgs 10 | { 11 | 12 | class GeoPoint : public ros::Msg 13 | { 14 | public: 15 | typedef float _latitude_type; 16 | _latitude_type latitude; 17 | typedef float _longitude_type; 18 | _longitude_type longitude; 19 | typedef float _altitude_type; 20 | _altitude_type altitude; 21 | 22 | GeoPoint(): 23 | latitude(0), 24 | longitude(0), 25 | altitude(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | offset += serializeAvrFloat64(outbuffer + offset, this->latitude); 33 | offset += serializeAvrFloat64(outbuffer + offset, this->longitude); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->altitude); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->latitude)); 42 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->longitude)); 43 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->altitude)); 44 | return offset; 45 | } 46 | 47 | const char * getType(){ return "geographic_msgs/GeoPoint"; }; 48 | const char * getMD5(){ return "c48027a852aeff972be80478ff38e81a"; }; 49 | 50 | }; 51 | 52 | } 53 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geographic_msgs/GeoPointStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geographic_msgs_GeoPointStamped_h 2 | #define _ROS_geographic_msgs_GeoPointStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geographic_msgs/GeoPoint.h" 10 | 11 | namespace geographic_msgs 12 | { 13 | 14 | class GeoPointStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geographic_msgs::GeoPoint _position_type; 20 | _position_type position; 21 | 22 | GeoPointStamped(): 23 | header(), 24 | position() 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->position.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->position.deserialize(inbuffer + offset); 41 | return offset; 42 | } 43 | 44 | const char * getType(){ return "geographic_msgs/GeoPointStamped"; }; 45 | const char * getMD5(){ return "ea50d268b03080563c330351a21edc89"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geographic_msgs/GeoPose.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geographic_msgs_GeoPose_h 2 | #define _ROS_geographic_msgs_GeoPose_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geographic_msgs/GeoPoint.h" 9 | #include "geometry_msgs/Quaternion.h" 10 | 11 | namespace geographic_msgs 12 | { 13 | 14 | class GeoPose : public ros::Msg 15 | { 16 | public: 17 | typedef geographic_msgs::GeoPoint _position_type; 18 | _position_type position; 19 | typedef geometry_msgs::Quaternion _orientation_type; 20 | _orientation_type orientation; 21 | 22 | GeoPose(): 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 "geographic_msgs/GeoPose"; }; 45 | const char * getMD5(){ return "778680b5172de58b7c057d973576c784"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geographic_msgs/GeoPoseStamped.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geographic_msgs_GeoPoseStamped_h 2 | #define _ROS_geographic_msgs_GeoPoseStamped_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | #include "geographic_msgs/GeoPose.h" 10 | 11 | namespace geographic_msgs 12 | { 13 | 14 | class GeoPoseStamped : public ros::Msg 15 | { 16 | public: 17 | typedef std_msgs::Header _header_type; 18 | _header_type header; 19 | typedef geographic_msgs::GeoPose _pose_type; 20 | _pose_type pose; 21 | 22 | GeoPoseStamped(): 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 "geographic_msgs/GeoPoseStamped"; }; 45 | const char * getMD5(){ return "cc409c8ed6064d8a846fa207bf3fba6b"; }; 46 | 47 | }; 48 | 49 | } 50 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/AccelWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_AccelWithCovariance_h 2 | #define _ROS_geometry_msgs_AccelWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Accel.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class AccelWithCovariance : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Accel _accel_type; 17 | _accel_type accel; 18 | float covariance[36]; 19 | 20 | AccelWithCovariance(): 21 | accel(), 22 | covariance() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->accel.serialize(outbuffer + offset); 30 | for( uint32_t i = 0; i < 36; i++){ 31 | offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->accel.deserialize(inbuffer + offset); 40 | for( uint32_t i = 0; i < 36; i++){ 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); 42 | } 43 | return offset; 44 | } 45 | 46 | const char * getType(){ return "geometry_msgs/AccelWithCovariance"; }; 47 | const char * getMD5(){ return "ad5a718d699c6be72a02b8d6a139f334"; }; 48 | 49 | }; 50 | 51 | } 52 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Point_h 2 | #define _ROS_geometry_msgs_Point_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace geometry_msgs 10 | { 11 | 12 | class Point : public ros::Msg 13 | { 14 | public: 15 | typedef float _x_type; 16 | _x_type x; 17 | typedef float _y_type; 18 | _y_type y; 19 | typedef float _z_type; 20 | _z_type z; 21 | 22 | Point(): 23 | x(0), 24 | y(0), 25 | z(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | offset += serializeAvrFloat64(outbuffer + offset, this->x); 33 | offset += serializeAvrFloat64(outbuffer + offset, this->y); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->z); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); 42 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); 43 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->z)); 44 | return offset; 45 | } 46 | 47 | const char * getType(){ return "geometry_msgs/Point"; }; 48 | const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; }; 49 | 50 | }; 51 | 52 | } 53 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/Pose2D.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Pose2D_h 2 | #define _ROS_geometry_msgs_Pose2D_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace geometry_msgs 10 | { 11 | 12 | class Pose2D : public ros::Msg 13 | { 14 | public: 15 | typedef float _x_type; 16 | _x_type x; 17 | typedef float _y_type; 18 | _y_type y; 19 | typedef float _theta_type; 20 | _theta_type theta; 21 | 22 | Pose2D(): 23 | x(0), 24 | y(0), 25 | theta(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | offset += serializeAvrFloat64(outbuffer + offset, this->x); 33 | offset += serializeAvrFloat64(outbuffer + offset, this->y); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->theta); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); 42 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); 43 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->theta)); 44 | return offset; 45 | } 46 | 47 | const char * getType(){ return "geometry_msgs/Pose2D"; }; 48 | const char * getMD5(){ return "938fa65709584ad8e77d238529be13b8"; }; 49 | 50 | }; 51 | 52 | } 53 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/PoseWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_PoseWithCovariance_h 2 | #define _ROS_geometry_msgs_PoseWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Pose.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class PoseWithCovariance : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Pose _pose_type; 17 | _pose_type pose; 18 | float covariance[36]; 19 | 20 | PoseWithCovariance(): 21 | pose(), 22 | covariance() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->pose.serialize(outbuffer + offset); 30 | for( uint32_t i = 0; i < 36; i++){ 31 | offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->pose.deserialize(inbuffer + offset); 40 | for( uint32_t i = 0; i < 36; i++){ 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); 42 | } 43 | return offset; 44 | } 45 | 46 | const char * getType(){ return "geometry_msgs/PoseWithCovariance"; }; 47 | const char * getMD5(){ return "c23e848cf1b7533a8d7c259073a97e6f"; }; 48 | 49 | }; 50 | 51 | } 52 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/TwistWithCovariance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_TwistWithCovariance_h 2 | #define _ROS_geometry_msgs_TwistWithCovariance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "geometry_msgs/Twist.h" 9 | 10 | namespace geometry_msgs 11 | { 12 | 13 | class TwistWithCovariance : public ros::Msg 14 | { 15 | public: 16 | typedef geometry_msgs::Twist _twist_type; 17 | _twist_type twist; 18 | float covariance[36]; 19 | 20 | TwistWithCovariance(): 21 | twist(), 22 | covariance() 23 | { 24 | } 25 | 26 | virtual int serialize(unsigned char *outbuffer) const 27 | { 28 | int offset = 0; 29 | offset += this->twist.serialize(outbuffer + offset); 30 | for( uint32_t i = 0; i < 36; i++){ 31 | offset += serializeAvrFloat64(outbuffer + offset, this->covariance[i]); 32 | } 33 | return offset; 34 | } 35 | 36 | virtual int deserialize(unsigned char *inbuffer) 37 | { 38 | int offset = 0; 39 | offset += this->twist.deserialize(inbuffer + offset); 40 | for( uint32_t i = 0; i < 36; i++){ 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->covariance[i])); 42 | } 43 | return offset; 44 | } 45 | 46 | const char * getType(){ return "geometry_msgs/TwistWithCovariance"; }; 47 | const char * getMD5(){ return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; }; 48 | 49 | }; 50 | 51 | } 52 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/geometry_msgs/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_geometry_msgs_Vector3_h 2 | #define _ROS_geometry_msgs_Vector3_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace geometry_msgs 10 | { 11 | 12 | class Vector3 : public ros::Msg 13 | { 14 | public: 15 | typedef float _x_type; 16 | _x_type x; 17 | typedef float _y_type; 18 | _y_type y; 19 | typedef float _z_type; 20 | _z_type z; 21 | 22 | Vector3(): 23 | x(0), 24 | y(0), 25 | z(0) 26 | { 27 | } 28 | 29 | virtual int serialize(unsigned char *outbuffer) const 30 | { 31 | int offset = 0; 32 | offset += serializeAvrFloat64(outbuffer + offset, this->x); 33 | offset += serializeAvrFloat64(outbuffer + offset, this->y); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->z); 35 | return offset; 36 | } 37 | 38 | virtual int deserialize(unsigned char *inbuffer) 39 | { 40 | int offset = 0; 41 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->x)); 42 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->y)); 43 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->z)); 44 | return offset; 45 | } 46 | 47 | const char * getType(){ return "geometry_msgs/Vector3"; }; 48 | const char * getMD5(){ return "4a842b65f413084dc2b10fb484ea7f17"; }; 49 | 50 | }; 51 | 52 | } 53 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/map_msgs/ProjectedMap.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_map_msgs_ProjectedMap_h 2 | #define _ROS_map_msgs_ProjectedMap_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "nav_msgs/OccupancyGrid.h" 9 | 10 | namespace map_msgs 11 | { 12 | 13 | class ProjectedMap : public ros::Msg 14 | { 15 | public: 16 | typedef nav_msgs::OccupancyGrid _map_type; 17 | _map_type map; 18 | typedef float _min_z_type; 19 | _min_z_type min_z; 20 | typedef float _max_z_type; 21 | _max_z_type max_z; 22 | 23 | ProjectedMap(): 24 | map(), 25 | min_z(0), 26 | max_z(0) 27 | { 28 | } 29 | 30 | virtual int serialize(unsigned char *outbuffer) const 31 | { 32 | int offset = 0; 33 | offset += this->map.serialize(outbuffer + offset); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->min_z); 35 | offset += serializeAvrFloat64(outbuffer + offset, this->max_z); 36 | return offset; 37 | } 38 | 39 | virtual int deserialize(unsigned char *inbuffer) 40 | { 41 | int offset = 0; 42 | offset += this->map.deserialize(inbuffer + offset); 43 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->min_z)); 44 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->max_z)); 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "map_msgs/ProjectedMap"; }; 49 | const char * getMD5(){ return "7bbe8f96e45089681dc1ea7d023cbfca"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/sensor_msgs/Illuminance.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_Illuminance_h 2 | #define _ROS_sensor_msgs_Illuminance_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class Illuminance : public ros::Msg 14 | { 15 | public: 16 | typedef std_msgs::Header _header_type; 17 | _header_type header; 18 | typedef float _illuminance_type; 19 | _illuminance_type illuminance; 20 | typedef float _variance_type; 21 | _variance_type variance; 22 | 23 | Illuminance(): 24 | header(), 25 | illuminance(0), 26 | variance(0) 27 | { 28 | } 29 | 30 | virtual int serialize(unsigned char *outbuffer) const 31 | { 32 | int offset = 0; 33 | offset += this->header.serialize(outbuffer + offset); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->illuminance); 35 | offset += serializeAvrFloat64(outbuffer + offset, this->variance); 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 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->illuminance)); 44 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "sensor_msgs/Illuminance"; }; 49 | const char * getMD5(){ return "8cf5febb0952fca9d650c3d11a81a188"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/sensor_msgs/Temperature.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_sensor_msgs_Temperature_h 2 | #define _ROS_sensor_msgs_Temperature_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | #include "std_msgs/Header.h" 9 | 10 | namespace sensor_msgs 11 | { 12 | 13 | class Temperature : public ros::Msg 14 | { 15 | public: 16 | typedef std_msgs::Header _header_type; 17 | _header_type header; 18 | typedef float _temperature_type; 19 | _temperature_type temperature; 20 | typedef float _variance_type; 21 | _variance_type variance; 22 | 23 | Temperature(): 24 | header(), 25 | temperature(0), 26 | variance(0) 27 | { 28 | } 29 | 30 | virtual int serialize(unsigned char *outbuffer) const 31 | { 32 | int offset = 0; 33 | offset += this->header.serialize(outbuffer + offset); 34 | offset += serializeAvrFloat64(outbuffer + offset, this->temperature); 35 | offset += serializeAvrFloat64(outbuffer + offset, this->variance); 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 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->temperature)); 44 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->variance)); 45 | return offset; 46 | } 47 | 48 | const char * getType(){ return "sensor_msgs/Temperature"; }; 49 | const char * getMD5(){ return "ff71b307acdbe7c871a5a6d7ed359100"; }; 50 | 51 | }; 52 | 53 | } 54 | #endif -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/shape_msgs/Plane.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_shape_msgs_Plane_h 2 | #define _ROS_shape_msgs_Plane_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace shape_msgs 10 | { 11 | 12 | class Plane : public ros::Msg 13 | { 14 | public: 15 | float coef[4]; 16 | 17 | Plane(): 18 | coef() 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | for( uint32_t i = 0; i < 4; i++){ 26 | offset += serializeAvrFloat64(outbuffer + offset, this->coef[i]); 27 | } 28 | return offset; 29 | } 30 | 31 | virtual int deserialize(unsigned char *inbuffer) 32 | { 33 | int offset = 0; 34 | for( uint32_t i = 0; i < 4; i++){ 35 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->coef[i])); 36 | } 37 | return offset; 38 | } 39 | 40 | const char * getType(){ return "shape_msgs/Plane"; }; 41 | const char * getMD5(){ return "2c1b92ed8f31492f8e73f6a4a44ca796"; }; 42 | 43 | }; 44 | 45 | } 46 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/std_msgs/Float64.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_std_msgs_Float64_h 2 | #define _ROS_std_msgs_Float64_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace std_msgs 10 | { 11 | 12 | class Float64 : public ros::Msg 13 | { 14 | public: 15 | typedef float _data_type; 16 | _data_type data; 17 | 18 | Float64(): 19 | data(0) 20 | { 21 | } 22 | 23 | virtual int serialize(unsigned char *outbuffer) const 24 | { 25 | int offset = 0; 26 | offset += serializeAvrFloat64(outbuffer + offset, this->data); 27 | return offset; 28 | } 29 | 30 | virtual int deserialize(unsigned char *inbuffer) 31 | { 32 | int offset = 0; 33 | offset += deserializeAvrFloat64(inbuffer + offset, &(this->data)); 34 | return offset; 35 | } 36 | 37 | const char * getType(){ return "std_msgs/Float64"; }; 38 | const char * getMD5(){ return "fdb28210bfa9d7c91146260178d9a584"; }; 39 | 40 | }; 41 | 42 | } 43 | #endif -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/tests/array_test/array_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::geometry_msgs::PoseArray Test 3 | * Sums an array, publishes sum 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | 14 | bool set_; 15 | 16 | 17 | geometry_msgs::Pose sum_msg; 18 | ros::Publisher p("sum", &sum_msg); 19 | 20 | void messageCb(const geometry_msgs::PoseArray& msg){ 21 | sum_msg.position.x = 0; 22 | sum_msg.position.y = 0; 23 | sum_msg.position.z = 0; 24 | for(int i = 0; i < msg.poses_length; i++) 25 | { 26 | sum_msg.position.x += msg.poses[i].position.x; 27 | sum_msg.position.y += msg.poses[i].position.y; 28 | sum_msg.position.z += msg.poses[i].position.z; 29 | } 30 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 31 | } 32 | 33 | ros::Subscriber s("poses",messageCb); 34 | 35 | void setup() 36 | { 37 | pinMode(13, OUTPUT); 38 | nh.initNode(); 39 | nh.subscribe(s); 40 | nh.advertise(p); 41 | } 42 | 43 | void loop() 44 | { 45 | p.publish(&sum_msg); 46 | nh.spinOnce(); 47 | delay(10); 48 | } 49 | 50 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/tests/float64_test/float64_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Float64 Test 3 | * Receives a Float64 input, subtracts 1.0, and publishes it 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | 10 | ros::NodeHandle nh; 11 | 12 | float x; 13 | 14 | void messageCb( const std_msgs::Float64& msg){ 15 | x = msg.data - 1.0; 16 | digitalWrite(13, HIGH-digitalRead(13)); // blink the led 17 | } 18 | 19 | std_msgs::Float64 test; 20 | ros::Subscriber s("your_topic", &messageCb); 21 | ros::Publisher p("my_topic", &test); 22 | 23 | void setup() 24 | { 25 | pinMode(13, OUTPUT); 26 | nh.initNode(); 27 | nh.advertise(p); 28 | nh.subscribe(s); 29 | } 30 | 31 | void loop() 32 | { 33 | test.data = x; 34 | p.publish( &test ); 35 | nh.spinOnce(); 36 | delay(10); 37 | } 38 | 39 | -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/tests/time_test/time_test.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial::std_msgs::Time Test 3 | * Publishes current time 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | ros::NodeHandle nh; 12 | 13 | std_msgs::Time test; 14 | ros::Publisher p("my_topic", &test); 15 | 16 | void setup() 17 | { 18 | pinMode(13, OUTPUT); 19 | nh.initNode(); 20 | nh.advertise(p); 21 | } 22 | 23 | void loop() 24 | { 25 | test.data = nh.now(); 26 | p.publish( &test ); 27 | nh.spinOnce(); 28 | delay(10); 29 | } 30 | 31 | -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/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 -------------------------------------------------------------------------------- /arduino/libraries/ros_lib/uuid_msgs/UniqueID.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_uuid_msgs_UniqueID_h 2 | #define _ROS_uuid_msgs_UniqueID_h 3 | 4 | #include 5 | #include 6 | #include 7 | #include "ros/msg.h" 8 | 9 | namespace uuid_msgs 10 | { 11 | 12 | class UniqueID : public ros::Msg 13 | { 14 | public: 15 | uint8_t uuid[16]; 16 | 17 | UniqueID(): 18 | uuid() 19 | { 20 | } 21 | 22 | virtual int serialize(unsigned char *outbuffer) const 23 | { 24 | int offset = 0; 25 | for( uint32_t i = 0; i < 16; i++){ 26 | *(outbuffer + offset + 0) = (this->uuid[i] >> (8 * 0)) & 0xFF; 27 | offset += sizeof(this->uuid[i]); 28 | } 29 | return offset; 30 | } 31 | 32 | virtual int deserialize(unsigned char *inbuffer) 33 | { 34 | int offset = 0; 35 | for( uint32_t i = 0; i < 16; i++){ 36 | this->uuid[i] = ((uint8_t) (*(inbuffer + offset))); 37 | offset += sizeof(this->uuid[i]); 38 | } 39 | return offset; 40 | } 41 | 42 | const char * getType(){ return "uuid_msgs/UniqueID"; }; 43 | const char * getMD5(){ return "fec2a93b6f5367ee8112c9c0b41ff310"; }; 44 | 45 | }; 46 | 47 | } 48 | #endif -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoDebugDataInput.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoDebugDataInput.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoDebugDataOutput.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoDebugDataOutput.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoDebugFingersOpen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoDebugFingersOpen.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoDebugHighCenter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoDebugHighCenter.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoDebugWristDown.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoDebugWristDown.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDEBoardType.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDEBoardType.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDELineEndingBaudRate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDELineEndingBaudRate.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDEOpenSketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDEOpenSketch.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDEOpenSketch2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDEOpenSketch2.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDESerialMonitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDESerialMonitor.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDESerialPort.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDESerialPort.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDESketchbookLocation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDESketchbookLocation.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDEUploadSketch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDEUploadSketch.png -------------------------------------------------------------------------------- /arduino/readmeImages/ArduinoIDEUploadSuccess.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/arduino/readmeImages/ArduinoIDEUploadSuccess.png -------------------------------------------------------------------------------- /camera_info/head_camera.yaml: -------------------------------------------------------------------------------- 1 | image_width: 320 2 | image_height: 240 3 | camera_name: head_camera 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [376.711141, 0.000000, 159.282944, 0.000000, 376.065748, 119.710353, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.128368, -0.726078, 0.006426, -0.002841, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [375.376709, 0.000000, 158.129592, 0.000000, 0.000000, 376.681763, 119.967005, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /cleanup.sh: -------------------------------------------------------------------------------- 1 | pkill gzclient 2 | echo "Killing gzclient" 3 | pkill gzserver 4 | echo "Killing gzserver" 5 | # pkill rqt 6 | # echo "Killing rqt" 7 | # sleep 1 8 | # rosnode kill -a 9 | pkill roslaunch 10 | echo "Killing roslaunch" 11 | sleep 1 12 | # pkill rosmaster 13 | # echo "Killing rosmaster" 14 | # sleep 1 15 | # pkill roscore 16 | # echo "Killing roscore" 17 | # sleep 1 18 | -------------------------------------------------------------------------------- /launch/.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017 G. Matthew Fricke, Josh P. Hecker, Antonio D. Griego, Jarett C. Jones, J. Jake Nichol, Karl Stollis, Kelsey Geiger, Tobi Ogunyale, Manny Meraz, Jeff Schlindwein, Will Vining, and Melanie E. Moses. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /misc/gen_rover_models.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Regenerate the model SDF and config files for the 8 default rovers. 3 | # This script should be executed any time the master model template is edited. 4 | 5 | declare -A rovers 6 | rovers=( 7 | ["achilles"]="FlatBlack" 8 | ["aeneas"]="Yellow" 9 | ["ajax"]="White" 10 | ["diomedes"]="Red" 11 | ["hector"]="Blue" 12 | ["paris"]="Orange" 13 | ["thor"]="Turquoise" 14 | ["zeus"]="Indigo" 15 | ) 16 | 17 | model_path=$(catkin locate)/simulation/models 18 | master_template_model=${model_path}/swarmie/model.sdf.erb 19 | master_template_config=${model_path}/swarmie/model.config.erb 20 | 21 | for rover in "${!rovers[@]}"; do 22 | erb -T - rovername=${rover} \ 23 | -- ${master_template_config} > ${model_path}/${rover}/model.config 24 | erb -T - rovername=${rover} rovercolor=${rovers[$rover]} \ 25 | -- ${master_template_model} > ${model_path}/${rover}/model.sdf 26 | done 27 | 28 | -------------------------------------------------------------------------------- /misc/get_arduino_port.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # START OF GET ARDUINO PORT FUNCTION 3 | get_arduino_port () { 4 | for sysdevpath in $(find /sys/bus/usb/devices/usb*/ -name dev); do 5 | ( 6 | syspath="${sysdevpath%/dev}" 7 | devname="$(udevadm info -q name -p $syspath)" 8 | [[ "$devname" == "bus/"* ]] && continue 9 | eval "$(udevadm info -q property --export -p $syspath)" 10 | [[ -z "$ID_SERIAL" ]] && continue 11 | 12 | if [[ "$ID_SERIAL" = *"Arduino"* ]]; then 13 | echo "/dev/$devname" 14 | fi 15 | ) 16 | done 17 | } 18 | get_arduino_port 19 | # END OF GET ARDUINO PORT FUNTION 20 | -------------------------------------------------------------------------------- /misc/logs/empty.txt: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /misc/nohup.out: -------------------------------------------------------------------------------- 1 | [rospack] Error: package 'obstacle_detection' not found 2 | [rospack] Error: package 'obstacle_detection' not found 3 | -------------------------------------------------------------------------------- /misc/rosbridge/simple-node.js: -------------------------------------------------------------------------------- 1 | ROSLIB = require('roslib'); 2 | var swarmie = require('./swarmie.js'); 3 | 4 | var rover = swarmie.make_swarmie(process.argv[2]); 5 | -------------------------------------------------------------------------------- /misc/rosbridge/swarmie-web.js: -------------------------------------------------------------------------------- 1 | 2 | // Connecting to ROS 3 | // ----------------- 4 | window.addEventListener('load', eventWindowLoaded, false); 5 | 6 | function eventWindowLoaded () { 7 | runSwarmieWebsite(); 8 | } 9 | 10 | function runSwarmieWebsite() { 11 | 12 | var urlParams = new URLSearchParams(window.location.search); 13 | 14 | var Name = urlParams.get('robotname'); 15 | console.log(Name); 16 | var swarmie = new Swarmie(Name); 17 | 18 | document.title = Name; 19 | }; 20 | -------------------------------------------------------------------------------- /misc/rover_launch_local.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | OPTSTRING=`getopt -l frame-rate: -- $0 $@` 3 | frame_rate=10 4 | eval set -- "$OPTSTRING" 5 | while true 6 | do 7 | case "$1" in 8 | --frame-rate ) frame_rate="$2"; shift 2 ;; 9 | -- ) shift ; break ;; 10 | esac 11 | done 12 | roscore & 13 | sleep 10 # give roscore enough time to start 14 | ./rover_onboard_node_launch.sh --frame-rate $frame_rate localhost $1 15 | -------------------------------------------------------------------------------- /misc/run_calibration.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [ -z $1 ]; then 3 | echo "Usage: ./run_calibration.sh location" 4 | exit 1 5 | fi 6 | pkill roscore 7 | arduino_port=$(./get_arduino_port.sh) 8 | if [ -z $arduino_port ]; then 9 | echo "No Ardunios found" 10 | exit 1 11 | else 12 | echo "Found Arduino on port $arduino_port" 13 | fi 14 | 15 | # Load the calibration Arduino sketch 16 | repo_path=`eval "cd $dir;pwd;cd - > /dev/null"` 17 | full_path=repo_path 18 | ~/arduino-1.8.5/arduino --upload --preserve-temp-files --pref serial.port=$arduino_port --pref build.verbose=true --pref upload.verbose=true --pref build.path=/tmp --pref sketchbook.path=$(realpath $PWD/../arduino) --pref board=leonardo $(realpath $PWD/../arduino/calibrator/calibrator.ino) 19 | 20 | #Start ROS core for ros serial 21 | roscore & 22 | sleep 10 23 | rosrun rosserial_python serial_node.py _baud:=9600 $arduino_port & 24 | sleep 1 25 | 26 | echo "Starting Calibration" 27 | echo "Press Ctrl+C (^C) when finished..." 28 | 29 | python calibrate.py $1 30 | pkill roscore 31 | exit 0 32 | -------------------------------------------------------------------------------- /misc/start_bridge.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | robots="" 4 | 5 | for robot in $@ 6 | do 7 | robots=$robots" -new-tab -url file://$PWD/rosbridge/robot_interface.html?robotname=$robot" 8 | done 9 | 10 | firefox $robots 11 | -------------------------------------------------------------------------------- /misc/stop_robots.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | proceed="" 4 | 5 | warn() 6 | { 7 | echo "WARNING: $1 is not connected to this computer!" 8 | echo "ARE YOU SURE YOU WANT TO PROCEED (y/n)?" 9 | read proceed 10 | } 11 | 12 | connected() 13 | { 14 | if [ -z "`rosnode list | grep -i "^/$1"`" ] 15 | then 16 | warn $1 17 | else 18 | proceed="y" 19 | fi 20 | } 21 | 22 | for robot in $@ 23 | do 24 | connected $robot 25 | if [ "$proceed" = "y" ] 26 | then 27 | ssh swarmie@$robot 'sudo reboot' 28 | fi 29 | proceed="n" 30 | done -------------------------------------------------------------------------------- /readmeImages/InstallGraphics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/InstallGraphics.png -------------------------------------------------------------------------------- /readmeImages/activeRovers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/activeRovers.png -------------------------------------------------------------------------------- /readmeImages/buildSim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/buildSim.png -------------------------------------------------------------------------------- /readmeImages/guiFirstScreen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/guiFirstScreen.png -------------------------------------------------------------------------------- /readmeImages/roverSensorOutputs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/roverSensorOutputs.png -------------------------------------------------------------------------------- /readmeImages/sensorDisplayTab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/sensorDisplayTab.png -------------------------------------------------------------------------------- /readmeImages/simControlTab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/simControlTab.png -------------------------------------------------------------------------------- /readmeImages/taskStatusTab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/readmeImages/taskStatusTab.png -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "Running in $PWD" 3 | previous_gazebo_model_path=${GAZEBO_MODEL_PATH} 4 | previous_gazebo_plugin_path=${GAZEBO_PLUGIN_PATH} 5 | export SWARMATHON_APP_ROOT="$PWD" 6 | export GAZEBO_MODEL_PATH="$PWD/simulation/models" 7 | export GAZEBO_PLUGIN_PATH="$PWD/build/gazebo_plugins" 8 | source "$PWD/devel/setup.bash" 9 | echo Cleaning up ROS and Gazebo Processes 10 | 11 | #Delete the rqt cache - can take 24 hours for changes in the UI 12 | # to show up otherwise 13 | rm ~/.config/ros.org/rqt_gui.ini 14 | 15 | ./cleanup.sh 16 | echo Killing rosmaster 17 | pkill rosmaster 18 | echo Killing roscore 19 | pkill roscore 20 | roscore & 21 | sleep 2 22 | roslaunch rosbridge_server rosbridge_websocket.launch & 23 | rqt -s rqt_rover_gui 24 | # The rover program cleans up after itself but if there is a crash this helps to make sure there are no leftovers 25 | echo Cleaning up ROS and Gazebo Processes 26 | rosnode kill -a 27 | echo Killing rosmaster 28 | pkill rosmaster 29 | echo Killing roscore 30 | pkill roscore 31 | ./cleanup.sh 32 | # Restore previous environment 33 | export GAZEBO_MODEL_PATH=$previous_gazebo_model_path 34 | export GAZEBO_PLUGIN_PATH=$previous_gazebo_plugin_path 35 | -------------------------------------------------------------------------------- /run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Handle user early termination through SIGINT (or ctrl-c) 4 | function userExit() 5 | { 6 | echo "Received ctrl-c or interrupt. Exiting." 7 | ./cleanup.sh 8 | # Kill processes in our process group (our children) 9 | kill -- -$$ 10 | 11 | } 12 | 13 | 14 | checkIfNodesAreRunning() 15 | { 16 | # Check that the expected nodes are running 17 | echo "check nodes are running" 18 | #/achilles_APRILTAG 19 | #/achilles_BASE2CAM 20 | checkIfNodeIsRunning /achilles_BEHAVIOUR 21 | #/achilles_MAP 22 | #/achilles_NAVSAT 23 | #/achilles_ODOM 24 | #/gazebo 25 | } 26 | 27 | checkIfNodeIsRunning() 28 | { 29 | local node_name=$1 30 | if [[ $(rosnode ping $node_name) = *"unknown node"* ]]; then 31 | echo "Check failed: $node_name is not running." 32 | exit 1 33 | fi 34 | } 35 | 36 | # Trap SIGINT and call userExit 37 | trap userExit SIGINT 38 | 39 | 40 | # Run the headless simulation see sun_headless_sim.sh usage output for parameter definitions 41 | setsid ./run_headless_sim.sh simulation/worlds/powerlaw_targets_example.world 3 /dev/null 2 42 & 42 | 43 | sleep 30 44 | 45 | checkIfNodesAreRunning 46 | 47 | 48 | -------------------------------------------------------------------------------- /simulation/launch/swarmathon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /simulation/models/achilles/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | achilles 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/aeneas/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | aeneas 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/ajax/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | ajax 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/at0/materials/scripts/at0.material: -------------------------------------------------------------------------------- 1 | material at0/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture atag-0.png 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/at0/materials/textures/atag-0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/at0/materials/textures/atag-0.png -------------------------------------------------------------------------------- /simulation/models/at0/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | at0 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Karl Stolleis 10 | 11 | 12 | 13 | April Tag 36h11 Tag 0 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/at0/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 0.01 8 | 9 | 0.00004 10 | 0.0 11 | 0.0 12 | 0.00004 13 | 0.0 14 | 0.00004 15 | 16 | 17 | 18 | 19 | 20 | 0.05 0.05 0.05 21 | 22 | 23 | 24 | 29 | 30 | 31 | 32 | 33 | 34 | 0.05 0.05 0.05 35 | 36 | 37 | 38 | 39 | 40 | 0.8 41 | 0.7 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /simulation/models/at1/materials/scripts/at1.material: -------------------------------------------------------------------------------- 1 | material at1/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture atag-1.png 12 | scale 1 1 13 | } 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /simulation/models/at1/materials/textures/atag-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/at1/materials/textures/atag-1.png -------------------------------------------------------------------------------- /simulation/models/at1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | at1 5 | 1.0 6 | model.sdf 7 | 8 | 9 | 10 | 11 | 12 | 13 | April Tag 36h11 Tag 1 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/at1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0.05 0.05 0.001 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_final_round/materials/scripts/at1.material: -------------------------------------------------------------------------------- 1 | material at1_barrier_final_round/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture atag-1.png 12 | scale 0.0020833333 0.3333333 13 | } 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_final_round/materials/textures/atag-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/at1_barrier_final_round/materials/textures/atag-1.png -------------------------------------------------------------------------------- /simulation/models/at1_barrier_final_round/model.config: -------------------------------------------------------------------------------- 1 | at1_barrier_final_round 2 | 1.0 3 | model.sdf 4 | 5 | Arena Barrier for Final Rounds 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_final_round/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0.15 24 0.001 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_prelim_round/materials/scripts/at1.material: -------------------------------------------------------------------------------- 1 | material at1_barrier_prelim_round/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture atag-1.png 12 | scale 0.003125 0.3333333 13 | } 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_prelim_round/materials/textures/atag-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/at1_barrier_prelim_round/materials/textures/atag-1.png -------------------------------------------------------------------------------- /simulation/models/at1_barrier_prelim_round/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | at1_barrier_prelim_round 5 | 1.0 6 | model.sdf 7 | 8 | 9 | 10 | 11 | 12 | 13 | April Tag 36h11 Tag 1 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/at1_barrier_prelim_round/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0.15 16 0.001 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /simulation/models/barrier_final_round/model.config: -------------------------------------------------------------------------------- 1 | barrier_final_round 2 | 1.0 3 | model.sdf 4 | 5 | Arena Barrier for Final Rounds 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/barrier_final_round/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 1.0 8 | 9 | 0.083 10 | 0 11 | 0 12 | 0 13 | 0 14 | 0.083 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.05 23.1 0.33 22 | 23 | 24 | 25 | 26 | 27 | 1 28 | 1 29 | 0.0 30 | 0.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 0.05 23.1 0.33 40 | 41 | 42 | Set the barrier colour to bright orange 43 | 44 | 255 131 0 1 45 | 255 131 0 1 46 | 0.1 0.1 0.1 1 47 | 0 0 0 0 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /simulation/models/barrier_prelim_round/model.config: -------------------------------------------------------------------------------- 1 | barrier_prelim_round 2 | 1.0 3 | model.sdf 4 | 5 | Arena Barrier for Preliminary Rounds 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/barrier_prelim_round/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 1.0 8 | 9 | 0.083 10 | 0 11 | 0 12 | 0 13 | 0 14 | 0.083 15 | 16 | 17 | 18 | 19 | 20 | 21 | 0.05 15 0.33 22 | 23 | 24 | 25 | 26 | 27 | 1 28 | 1 29 | 0.0 30 | 0.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 0.05 15 0.33 40 | 41 | 42 | Set the barrier colour to bright orange 43 | 44 | 255 131 0 1 45 | 255 131 0 1 46 | 0.1 0.1 0.1 1 47 | 0 0 0 0 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /simulation/models/boulder1/boulder1.material: -------------------------------------------------------------------------------- 1 | material boulder1/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture boulder1.png 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/boulder1/boulder1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/boulder1/boulder1.png -------------------------------------------------------------------------------- /simulation/models/boulder1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | boulder1 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/boulder2/boulder2.material: -------------------------------------------------------------------------------- 1 | material boulder2/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture boulder2.png 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/boulder2/boulder2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/boulder2/boulder2.png -------------------------------------------------------------------------------- /simulation/models/boulder2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | boulder2 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/box/materials/scripts/box.material: -------------------------------------------------------------------------------- 1 | material box/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture boxTexture.jpg 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/box/materials/textures/boxTexture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/box/materials/textures/boxTexture.jpg -------------------------------------------------------------------------------- /simulation/models/box/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | box 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Antonio Griego 9 | 10 | 11 | 12 | a simple box obstacle 13 | 14 | 15 | -------------------------------------------------------------------------------- /simulation/models/box/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.0 0.0 0.25 0.0 0.0 0.0 5 | true 6 | 7 | 8 | 0.01 9 | 10 | 0.00004 11 | 0.0 12 | 0.0 13 | 0.00004 14 | 0.0 15 | 0.00004 16 | 17 | 18 | 19 | 20 | 21 | 0.5 0.5 0.5 22 | 23 | 24 | 25 | 30 | 31 | 32 | 33 | 34 | 35 | 0.5 0.5 0.5 36 | 37 | 38 | 39 | 40 | 41 | 0.8 42 | 0.7 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /simulation/models/carpark_ground_plane/model.config: -------------------------------------------------------------------------------- 1 | Carpark Ground Plane 2 | 1.0 3 | model.sdf 4 | 5 | Carpark textured ground plane. 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/carpark_ground_plane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /simulation/models/cinder_block/model.config: -------------------------------------------------------------------------------- 1 | Cinder Block 2 | 1.0 3 | model.sdf 4 | 5 | Cinder block. 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/collection_disk/materials/scripts/collection_disk.material: -------------------------------------------------------------------------------- 1 | material collection_disk/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting off 8 | alpha_rejection greater_equal 128 9 | 10 | texture_unit 11 | { 12 | texture collection_disk.png 13 | } 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /simulation/models/collection_disk/materials/textures/collection_disk.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/collection_disk/materials/textures/collection_disk.pdf -------------------------------------------------------------------------------- /simulation/models/collection_disk/materials/textures/collection_disk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/collection_disk/materials/textures/collection_disk.png -------------------------------------------------------------------------------- /simulation/models/collection_disk/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | collection_zone 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Jake Nichol 10 | 11 | 12 | 13 | Zone for collecting tags in the arena. 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/collection_disk/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 1.016 1.016 0.001 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | /collectionZone/score 25 | 26 | 1.016 27 | 28 | 0.2 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /simulation/models/concrete_ground_plane/model.config: -------------------------------------------------------------------------------- 1 | Concrete Ground Plane 2 | 1.0 3 | model.sdf 4 | 5 | Concrete textured ground plane. 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/concrete_ground_plane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /simulation/models/diomedes/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | diomedes 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/hector/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/lunar_lander/lunar_lander.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/lunar_lander/lunar_lander.stl -------------------------------------------------------------------------------- /simulation/models/lunar_lander/lunar_lander_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/lunar_lander/lunar_lander_base.stl -------------------------------------------------------------------------------- /simulation/models/lunar_lander/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | lunar_lander 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/mars_ground_plane/model.config: -------------------------------------------------------------------------------- 1 | Mars Ground Plane 2 | 1.0 3 | model.sdf 4 | 5 | Mars textured ground plane. 6 | 7 | 8 | -------------------------------------------------------------------------------- /simulation/models/mars_ground_plane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 100 100 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/brick.material: -------------------------------------------------------------------------------- 1 | material Brick/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | scale 0.05 0.05 11 | texture brick.png 12 | filtering anistropic 13 | max_anisotropy 16 14 | } 15 | 16 | } 17 | } 18 | } 19 | 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/carpark_ground_plane.material: -------------------------------------------------------------------------------- 1 | material CarparkGroundPlane/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 0.5 0.5 0.5 1.0 8 | diffuse 1.0 1.0 1.0 1.0 9 | specular 0.0 0.0 0.0 1.0 0.5 10 | 11 | texture_unit 12 | { 13 | texture carpark.jpg 14 | filtering trilinear 15 | scale 0.1 0.1 16 | } 17 | } 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/collection_disk.material: -------------------------------------------------------------------------------- 1 | material collection_disk/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 0.5 0.5 0.5 1.0 8 | diffuse 1.0 1.0 1.0 1.0 9 | specular 0.0 0.0 0.0 1.0 0.5 10 | 11 | texture_unit 12 | { 13 | texture collection_disk.png 14 | filtering trilinear 15 | scale 0.1 0.1 16 | } 17 | } 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/concrete_ground_plane.material: -------------------------------------------------------------------------------- 1 | material ConcreteGroundPlane/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 0.5 0.5 0.5 1.0 8 | diffuse 1.0 1.0 1.0 1.0 9 | specular 0.0 0.0 0.0 1.0 0.5 10 | 11 | texture_unit 12 | { 13 | texture concreteTile.jpg 14 | filtering trilinear 15 | scale 0.01 0.01 16 | } 17 | } 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/mars_ground_plane.material: -------------------------------------------------------------------------------- 1 | material MarsGroundPlane/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 0.5 0.5 0.5 1.0 8 | diffuse 1.0 1.0 1.0 1.0 9 | specular 0.0 0.0 0.0 1.0 0.5 10 | 11 | texture_unit 12 | { 13 | texture GravelImage.png 14 | filtering trilinear 15 | scale 0.02 0.02 16 | } 17 | } 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/scripts/white.material: -------------------------------------------------------------------------------- 1 | material Brick/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | scale 0.05 0.05 11 | texture white.png 12 | filtering anistropic 13 | max_anisotropy 16 14 | } 15 | 16 | } 17 | } 18 | } 19 | 20 | -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/GravelImage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/GravelImage.png -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/GravelImage_old.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/GravelImage_old.png -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/asphalt.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/asphalt.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/carpark.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/carpark.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/carpark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/carpark.png -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/carpark2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/carpark2.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/collection_disk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/collection_disk.png -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/concrete.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/concrete.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/concrete2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/concrete2.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/concreteTile.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/concreteTile.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/ksc_carpark2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/ksc_carpark2.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/ksc_carpark3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/ksc_carpark3.jpg -------------------------------------------------------------------------------- /simulation/models/media/materials/textures/white.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/media/materials/textures/white.png -------------------------------------------------------------------------------- /simulation/models/paris/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | paris 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/red_sky_filter/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | red_sky_filter 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/rock_barrier/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | rock_barrier 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/rock_barrier/rock_barrier.material: -------------------------------------------------------------------------------- 1 | material rock_barrier/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture rock_barrier.png 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/rock_barrier/rock_barrier.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/rock_barrier/rock_barrier.png -------------------------------------------------------------------------------- /simulation/models/rock_barrier/rock_barrier.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/rock_barrier/rock_barrier.stl -------------------------------------------------------------------------------- /simulation/models/rock_bridge/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | rock_bridge 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/models/rock_bridge/rock_bridge.material: -------------------------------------------------------------------------------- 1 | material rock_bridge/Diffuse 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | transparent_sorting on 8 | 9 | texture_unit 10 | { 11 | texture rock_bridge.png 12 | } 13 | } 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /simulation/models/rock_bridge/rock_bridge.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/rock_bridge/rock_bridge.png -------------------------------------------------------------------------------- /simulation/models/swarmie/README.md: -------------------------------------------------------------------------------- 1 | To regenerate a rover's model.sdf from the erb template: 2 | ``` 3 | erb -T - rovername=name rovercolor=color -- model.sdf.erb > model.sdf 4 | ``` 5 | 6 | NOTE: You should use `misc/gen_rover_models.sh` to generate the all of the default 7 | rover models. Before you can use the script the catkin workspace has to be configured (catkin build). 8 | -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/Body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/Body.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/CoverPlate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/CoverPlate.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/CowCatcherFront.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/CowCatcherFront.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/CowCatcherRear.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/CowCatcherRear.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/GripperFingerLeft.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/GripperFingerLeft.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/GripperFingerRight.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/GripperFingerRight.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/GripperWrist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/GripperWrist.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/TireWheelLeft.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/TireWheelLeft.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/meshes/TireWheelRight.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/simulation/models/swarmie/meshes/TireWheelRight.stl -------------------------------------------------------------------------------- /simulation/models/swarmie/model.config.erb: -------------------------------------------------------------------------------- 1 | 2 | <%- 3 | # This is the master rover model config template. Any updates to the 4 | # configuration should be make to this file before regenerating the file for 5 | # each rover. 6 | 7 | if !defined?(rovername) 8 | STDERR.puts "Usage: erb -T - rovername=name -- " + __FILE__ 9 | exit(false) 10 | end 11 | -%> 12 | 13 | <%= rovername %> 14 | 1.0 15 | model.sdf 16 | 17 | 18 | NASA Swarmathon 19 | swarmathon@cs.unm.edu 20 | 21 | 22 | 23 | Swarmie Version Three 24 | 25 | 26 | -------------------------------------------------------------------------------- /simulation/models/thor/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | thor 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/models/zeus/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | zeus 4 | 1.0 5 | model.sdf 6 | 7 | 8 | NASA Swarmathon 9 | swarmathon@cs.unm.edu 10 | 11 | 12 | 13 | Swarmie Version Three 14 | 15 | 16 | -------------------------------------------------------------------------------- /simulation/worlds/swarmathon.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/.idea/.name: -------------------------------------------------------------------------------- 1 | src -------------------------------------------------------------------------------- /src/.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/abridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(abridge) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | sensor_msgs 8 | std_msgs 9 | tf 10 | nav_msgs 11 | swarmie_msgs 12 | ) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs tf nav_msgs swarmie_msgs 16 | ) 17 | 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} include 20 | ) 21 | 22 | add_executable( 23 | abridge src/abridge.cpp src/usbSerial.cpp 24 | ) 25 | 26 | target_link_libraries( 27 | abridge 28 | ${catkin_LIBRARIES} 29 | ) 30 | 31 | -------------------------------------------------------------------------------- /src/abridge/include/usbSerial.h: -------------------------------------------------------------------------------- 1 | #ifndef USBSERIAL_H 2 | #define USBSERIAL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | class USBSerial { 16 | public: 17 | 18 | USBSerial(); 19 | virtual ~USBSerial(); 20 | 21 | void openUSBPort(string device_path, int baud); 22 | void sendData(char data[]); 23 | string readData(); 24 | void closeUSBPort(); 25 | 26 | private: 27 | 28 | struct termios io_struct; 29 | int usb_file_descriptor; 30 | char serial_data_in[200]; 31 | char data_out[16]; 32 | 33 | }; 34 | 35 | #endif /* USBSERIAL_H */ 36 | 37 | -------------------------------------------------------------------------------- /src/abridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | abridge 4 | 0.2.0 5 | Arduino interface to provide low-level access to sensors and actuators 6 | 7 | NASA Swarmathon 8 | 9 | MIT 10 | 11 | catkin 12 | geometry_msgs 13 | roscpp 14 | sensor_msgs 15 | std_msgs 16 | tf 17 | nav_msgs 18 | swarmie_msgs 19 | 20 | geometry_msgs 21 | roscpp 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | nav_msgs 26 | swarmie_msgs 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/abridge/src/usbSerial.cpp: -------------------------------------------------------------------------------- 1 | #include "usbSerial.h" 2 | 3 | using namespace std; 4 | 5 | USBSerial::USBSerial() { 6 | 7 | } 8 | 9 | void USBSerial::openUSBPort(string device_path, int baud) 10 | { 11 | memset(&io_struct, 0, sizeof (io_struct)); 12 | io_struct.c_iflag = 0; 13 | io_struct.c_oflag = 0; 14 | io_struct.c_cflag = CS8 | CREAD | CLOCAL; 15 | io_struct.c_lflag = 0; 16 | io_struct.c_cc[VMIN] = 1; 17 | io_struct.c_cc[VTIME] = 5; 18 | 19 | usb_file_descriptor = open(device_path.c_str(), O_RDWR | O_NONBLOCK); 20 | if (usb_file_descriptor <= 0) 21 | { 22 | cout << "Opening USB0 FAILED " << usb_file_descriptor << endl; 23 | exit(1); 24 | } 25 | cfsetospeed(&io_struct, B115200); 26 | cfsetispeed(&io_struct, B115200); 27 | tcsetattr(usb_file_descriptor, TCSANOW, &io_struct); 28 | } 29 | 30 | void USBSerial::sendData(char data[]) 31 | { 32 | sprintf(data_out, "%s", data); 33 | write(usb_file_descriptor, data_out, sizeof (data_out)); 34 | memset(&data_out, '\0', sizeof (data_out)); 35 | } 36 | 37 | string USBSerial::readData() 38 | { 39 | if (read(usb_file_descriptor, &serial_data_in, sizeof (serial_data_in)) > 0) 40 | { 41 | read(usb_file_descriptor, &serial_data_in, sizeof (serial_data_in)); 42 | } 43 | string str(serial_data_in); 44 | tcflush(usb_file_descriptor, TCIOFLUSH); 45 | memset(&serial_data_in, '\0', sizeof (serial_data_in)); 46 | return str; 47 | } 48 | 49 | void USBSerial::closeUSBPort() 50 | { 51 | close(usb_file_descriptor); 52 | } 53 | 54 | USBSerial::~USBSerial() 55 | { 56 | closeUSBPort(); 57 | } 58 | 59 | -------------------------------------------------------------------------------- /src/behaviours/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(behaviours) 3 | 4 | SET(CMAKE_CXX_FLAGS "-std=c++11") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | geometry_msgs 8 | roscpp 9 | sensor_msgs 10 | std_msgs 11 | random_numbers 12 | tf 13 | apriltags_ros 14 | swarmie_msgs 15 | ) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS geometry_msgs swarmie_msgs roscpp sensor_msgs std_msgs random_numbers tf apriltags_ros 19 | ) 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | add_executable( 26 | behaviours 27 | src/Tag.cpp 28 | src/ObstacleController.cpp 29 | src/PickUpController.cpp 30 | src/DropOffController.cpp 31 | src/SearchController.cpp 32 | src/ROSAdapter.cpp 33 | src/PID.cpp 34 | src/DriveController.cpp 35 | src/RangeController.cpp 36 | src/LogicController.cpp 37 | src/ManualWaypointController.cpp 38 | src/PositionPublisher.cpp 39 | ) 40 | 41 | add_dependencies(behaviours ${catkin_EXPORTED_TARGETS}) 42 | 43 | target_link_libraries( 44 | behaviours 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | -------------------------------------------------------------------------------- /src/behaviours/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | behaviours 4 | 0.2.0 5 | Algorithmic implementation of random search as a finite state machine 6 | 7 | NASA Swarmathon 8 | 9 | MIT 10 | 11 | catkin 12 | geometry_msgs 13 | roscpp 14 | sensor_msgs 15 | std_msgs 16 | random_numbers 17 | tf 18 | apriltags_ros 19 | swarmie_msgs 20 | 21 | geometry_msgs 22 | roscpp 23 | sensor_msgs 24 | std_msgs 25 | random_numbers 26 | tf 27 | apriltags_ros 28 | swarmie_msgs 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/behaviours/src/Controller.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_H 2 | #define CONTROLLER_H 3 | 4 | #include "Result.h" 5 | 6 | /* 7 | * This class is meant to serve as a template for all Controllers, 8 | * including new Controllers defined by each team. 9 | * 10 | * It also exists so that Controllers have a generic interface to 11 | * use for processing interrupts in LogicController. 12 | */ 13 | class Controller { 14 | public: 15 | Controller() {} 16 | ~Controller() {} 17 | 18 | //Resets internal state to defaults 19 | virtual void Reset() = 0; 20 | 21 | //Determines what action should be taken based on current 22 | //internal state and data 23 | virtual Result DoWork() = 0; 24 | 25 | //Returns whether or not an interrupt must be thrown 26 | virtual bool ShouldInterrupt() = 0; 27 | 28 | //Returns whether or not a controller should be polled for a Result 29 | virtual bool HasWork() = 0; 30 | 31 | protected: 32 | 33 | //Looks at external data and determines if an interrupt must be thrown 34 | //or if the controller should be polled 35 | virtual void ProcessData() = 0; 36 | }; 37 | 38 | #endif // CONTROLLER_H 39 | -------------------------------------------------------------------------------- /src/behaviours/src/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | struct PIDConfig { 10 | float Kp = 0; 11 | float Ki = 0; 12 | float Kd = 0; 13 | float satUpper = 255; 14 | float satLower = -255; 15 | float antiWindup = satUpper/2; 16 | int errorHistLength = 4; 17 | bool alwaysIntegral = false; 18 | bool resetOnSetpoint = true; 19 | float feedForwardMultiplier = 0; 20 | float integralDeadZone = 0.01; 21 | float integralErrorHistoryLength = 10000; 22 | float integralMax = 255/2; 23 | float derivativeAlpha = 0.7; 24 | }; 25 | 26 | class PID 27 | { 28 | public: 29 | 30 | 31 | 32 | PIDConfig config; 33 | 34 | PID(); 35 | PID(PIDConfig config); 36 | 37 | float PIDOut(float calculatedError, float setPoint); 38 | 39 | void SetConfiguration(PIDConfig config) {this->config = config;} 40 | 41 | private: 42 | 43 | vector Error; 44 | float prevSetPoint = std::numeric_limits::min(); 45 | vector integralErrorHistArray; 46 | int step = 0; 47 | float hz = 10; //Rate that PID is running at 48 | 49 | }; 50 | 51 | #endif // PID_H 52 | -------------------------------------------------------------------------------- /src/behaviours/src/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_H 2 | #define POINT_H 3 | 4 | 5 | struct Point { 6 | float x; 7 | float y; 8 | float theta; 9 | }; 10 | 11 | #endif // POINT_H 12 | -------------------------------------------------------------------------------- /src/behaviours/src/PositionPublisher.cpp: -------------------------------------------------------------------------------- 1 | #include "PositionPublisher.hpp" 2 | #include "swarmie_msgs/Recruitment.h" 3 | 4 | #include 5 | 6 | PositionPublisher::PositionPublisher(ros::NodeHandle& node_handle, std::string hostname) 7 | { 8 | position_publisher = node_handle.advertise("/detectionLocations", 1); 9 | name.data = hostname; 10 | } 11 | 12 | void PositionPublisher::setDetections(std::vector tags, Point current_position) 13 | { 14 | int cube_tag_count = std::count_if(tags.begin(), tags.end(), 15 | [](Tag t) { return t.getID() == 0; }); 16 | if(cube_tag_count > 3) 17 | { 18 | // then there is definitely more than one cube 19 | swarmie_msgs::Recruitment r; 20 | r.x = current_position.x; 21 | r.y = current_position.y; 22 | r.name = name; 23 | position_publisher.publish(r); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/behaviours/src/PositionPublisher.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BEHAVIOR_POSITION_PUBLISHER_HPP 2 | #define _BEHAVIOR_POSITION_PUBLISHER_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "Point.h" 11 | #include "Tag.h" 12 | 13 | class PositionPublisher 14 | { 15 | private: 16 | ros::Publisher position_publisher; 17 | std_msgs::String name; 18 | 19 | public: 20 | PositionPublisher(ros::NodeHandle& node_handle, std::string hostname); 21 | void setDetections(std::vector tags, Point p); 22 | }; 23 | 24 | #endif // _BEHAVIOR_POSITION_PUBLISHER_HPP -------------------------------------------------------------------------------- /src/behaviours/src/SearchController.h: -------------------------------------------------------------------------------- 1 | #ifndef SEARCH_CONTROLLER 2 | #define SEARCH_CONTROLLER 3 | 4 | #include 5 | #include "Controller.h" 6 | 7 | /** 8 | * This class implements the search control algorithm for the rovers. The code 9 | * here should be modified and enhanced to improve search performance. 10 | */ 11 | class SearchController : virtual Controller { 12 | 13 | public: 14 | 15 | SearchController(); 16 | 17 | void Reset() override; 18 | 19 | // performs search pattern 20 | Result DoWork() override; 21 | bool ShouldInterrupt() override; 22 | bool HasWork() override; 23 | 24 | // sets the value of the current location 25 | //void UpdateData(geometry_msgs::Pose2D currentLocation, geometry_msgs::Pose2D centerLocation); 26 | void SetCurrentLocation(Point currentLocation); 27 | void SetCenterLocation(Point centerLocation); 28 | void SetSuccesfullPickup(); 29 | void setRecruitmentLocation(Point p); 30 | 31 | protected: 32 | 33 | void ProcessData(); 34 | 35 | private: 36 | 37 | random_numbers::RandomNumberGenerator* rng; 38 | Point currentLocation; 39 | Point centerLocation; 40 | Point searchLocation; 41 | int attemptCount = 0; 42 | //struct for returning data to ROS adapter 43 | Result result; 44 | 45 | // Search state 46 | // Flag to allow special behaviour for the first waypoint 47 | bool first_waypoint = true; 48 | bool succesfullPickup = false; 49 | }; 50 | 51 | #endif /* SEARCH_CONTROLLER */ 52 | -------------------------------------------------------------------------------- /src/diagnostics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(diagnostics) 3 | 4 | SET(CMAKE_CXX_FLAGS "-std=c++11") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | std_msgs 9 | gazebo_ros 10 | ) 11 | 12 | catkin_package( 13 | DEPENDS 14 | roscpp 15 | std_msgs 16 | gazebo_ros 17 | ) 18 | 19 | # Depend on system install of Gazebo 20 | find_package(gazebo REQUIRED) 21 | 22 | link_directories(${GAZEBO_LIBRARY_DIRS}) 23 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) 24 | 25 | add_executable( 26 | diagnostics 27 | src/driver.cpp 28 | src/Diagnostics.cpp 29 | src/WirelessDiags.cpp 30 | ) 31 | 32 | add_dependencies(diagnostics ${catkin_EXPORTED_TARGETS}) 33 | 34 | target_link_libraries( 35 | diagnostics 36 | usb 37 | ${catkin_LIBRARIES} 38 | ${GAZEBO_LIBRARIES} 39 | ) 40 | -------------------------------------------------------------------------------- /src/diagnostics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | diagnostics 4 | 0.0.0 5 | The diagnostics package 6 | NASA Swarmathon 7 | MIT 8 | catkin 9 | roscpp 10 | std_msgs 11 | gazebo_ros 12 | roscpp 13 | std_msgs 14 | gazebo_ros 15 | 16 | -------------------------------------------------------------------------------- /src/diagnostics/src/WirelessDiags.h: -------------------------------------------------------------------------------- 1 | #ifndef WirelessDiags_h 2 | #define WirelessDiags_h 3 | 4 | #include // wireless device interface name 5 | #include // gettimeofday and timeval 6 | 7 | //struct to hold collected information 8 | struct WirelessInfo { 9 | char mac[18]; 10 | char ssid[33]; 11 | int bandwidthAvailable; 12 | int level; 13 | int quality; 14 | int noise; 15 | float bandwidthUsed; 16 | }; 17 | 18 | class WirelessDiags { 19 | 20 | public: 21 | 22 | WirelessDiags(); 23 | 24 | // Sets the name of the interface 25 | // about which to provide information 26 | // returns the name of the wireless interface 27 | std::string setInterface(); 28 | 29 | WirelessInfo getInfo(); 30 | 31 | private: 32 | 33 | 34 | // We don't want to try and get info about a network interface that doesn't exist 35 | bool isInterfaceUp(std::string name); 36 | 37 | std::string findWirelessInterface(); 38 | 39 | // checks if a network interface is wireless or not 40 | bool isWireless(const char* name); 41 | float calcBitRate(); 42 | 43 | std::string interfaceName; 44 | 45 | // State needed to keep track of 46 | // the number of bytes sent between calcBitRate calls 47 | 48 | long int prev_total_bytes = 0; 49 | long int total_bytes = 0; 50 | struct timeval prev_time; // The wall time of the last call to calcBitRate; 51 | }; 52 | 53 | #endif // WirelessDiags_h 54 | -------------------------------------------------------------------------------- /src/diagnostics/src/driver.cpp: -------------------------------------------------------------------------------- 1 | // Driver for the diagnostics module. Provides an start point for the OS. 2 | // This driver's only job is to instantiate the Diagnostics class, handle ROS initialization, 3 | // allow ROS to handle events (spin), and determine the name to publish under (command line 4 | // argument or, if none, the hostname). 5 | // 6 | 7 | #include "Diagnostics.h" 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | // OS Signal Handler 15 | void sigintEventHandler(int signal); 16 | 17 | int main(int argc, char** argv) { 18 | 19 | sleep(10); 20 | char host[128]; 21 | gethostname(host, sizeof (host)); 22 | std::string hostname(host); 23 | 24 | std::string publishedName; 25 | if (argc >= 2) { 26 | publishedName = argv[1]; 27 | cout << "Welcome to the world of tomorrow " << publishedName << "! Diagnostic module started." << endl; 28 | } else { 29 | publishedName = hostname; 30 | cout << "No Name Selected. Default is: " << publishedName << endl; 31 | } 32 | 33 | // NoSignalHandler so we can catch SIGINT ourselves and shutdown the node 34 | ros::init(argc, argv, (publishedName + "_DIAGNOSTICS"), ros::init_options::NoSigintHandler); 35 | 36 | Diagnostics diags(publishedName); 37 | 38 | signal(SIGINT, sigintEventHandler); // Register the SIGINT event handler so the node can shutdown properly 39 | 40 | ros::spin(); // Process ROS events 41 | 42 | return EXIT_SUCCESS; 43 | } 44 | 45 | void sigintEventHandler(int sig) { 46 | // All the default sigint handler does is call shutdown() 47 | ros::shutdown(); 48 | } 49 | -------------------------------------------------------------------------------- /src/gazebo_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gazebo_plugins) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++0x") 5 | 6 | # Load catkin and all dependencies required for this package 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | gazebo_ros 10 | ) 11 | 12 | catkin_package( 13 | DEPENDS 14 | roscpp 15 | gazebo_ros 16 | ) 17 | 18 | # Depend on system install of Gazebo 19 | find_package(gazebo REQUIRED) 20 | 21 | link_directories(${GAZEBO_LIBRARY_DIRS}) 22 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} src src/GripperPlugin ${CMAKE_CURRENT_BINARY_DIR}) 23 | 24 | add_library(${PROJECT_NAME} src/SetupWorld.cpp) 25 | 26 | add_library(${PROJECT_NAME}_gripper 27 | src/GripperPlugin/GripperPlugin.cpp 28 | src/GripperPlugin/PIDController.cpp 29 | src/GripperPlugin/GripperManager.cpp) 30 | 31 | add_library(${PROJECT_NAME}_score 32 | src/ScorePlugin/ScorePlugin.cpp) 33 | 34 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 35 | 36 | -------------------------------------------------------------------------------- /src/gazebo_plugins/src/GripperPlugin/PIDController.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_CONTROLLER_H 2 | #define PID_CONTROLLER_H 3 | 4 | #include 5 | 6 | /** 7 | * This class defines the base PID controller to be used with all of the joints 8 | * in the GripperPlugin. 9 | * 10 | * @author Matthew Fricke 11 | * @author Antonio Griego 12 | */ 13 | class PIDController { 14 | 15 | public: 16 | 17 | // this struct defines all of the key variables for the PID controller and 18 | // simplifies passing around the entire series of values between functions 19 | struct PIDSettings { 20 | float Kp; 21 | float Ki; 22 | float Kd; 23 | float dt; 24 | float max; 25 | float min; 26 | }; 27 | 28 | // constructors 29 | PIDController(); 30 | PIDController(PIDController::PIDSettings settings); 31 | 32 | // returns a force to apply based on the setPoint vs. currentValue 33 | float update(float setPoint, float currentValue); 34 | 35 | private: 36 | 37 | float dt; // Update time 38 | float max; // Max setpoint 39 | float min; // Min setpoint 40 | float Kp; // Proportional gain 41 | float Kd; // Derivative gain 42 | float Ki; // Integral gain 43 | 44 | float previousError; // Previous error (starts at 0.0) 45 | float integral; // Sum of the error seen so far 46 | bool isInitialized; // PID controller status flag 47 | 48 | }; 49 | 50 | #endif /* PID_CONTROLLER_H */ 51 | -------------------------------------------------------------------------------- /src/gazebo_plugins/src/SetupWorld.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "gazebo/gazebo.hh" 3 | #include "gazebo/common/Plugin.hh" 4 | #include "gazebo/msgs/msgs.hh" 5 | #include "gazebo/physics/physics.hh" 6 | #include "gazebo/transport/transport.hh" 7 | #include 8 | 9 | using namespace std; 10 | 11 | namespace gazebo 12 | { 13 | class SetupWorld : public WorldPlugin 14 | { 15 | public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf) 16 | { 17 | cout << "Setting up world..." << flush; 18 | 19 | // Create a new transport node 20 | transport::NodePtr node(new transport::Node()); 21 | 22 | // Initialize the node with the world name 23 | node->Init(_parent->GetName()); 24 | 25 | // Create a publisher on the ~/physics topic 26 | transport::PublisherPtr physicsPub = 27 | node->Advertise("~/physics"); 28 | 29 | msgs::Physics physicsMsg; 30 | physicsMsg.set_type(msgs::Physics::ODE); 31 | 32 | // Set the step time 33 | physicsMsg.set_max_step_size(0.001); 34 | 35 | // Set the real time update rate 36 | physicsMsg.set_real_time_update_rate(0); 37 | 38 | // Change gravity 39 | //msgs::Set(physicsMsg.mutable_gravity(), math::Vector3(0.01, 0, 0.1)); 40 | 41 | physicsPub->Publish(physicsMsg); 42 | 43 | cout << " done." << endl; 44 | } 45 | }; 46 | 47 | // Register this plugin with the simulator 48 | GZ_REGISTER_WORLD_PLUGIN(SetupWorld) 49 | } 50 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/launch/rover_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rqt_rover_gui 3 | 0.0.1 4 | The Rover Interface 5 | 6 | NASA Swarmathon 7 | 8 | MIT 9 | 10 | catkin 11 | 12 | rqt_gui 13 | rqt_gui_cpp 14 | cv_bridge 15 | image_transport 16 | geometry_msgs 17 | ublox_serialization 18 | ublox_msgs 19 | swarmie_msgs 20 | 21 | rqt_gui 22 | rqt_gui_cpp 23 | cv_bridge 24 | image_transport 25 | geometry_msgs 26 | ublox_serialization 27 | ublox_msgs 28 | swarmie_msgs 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | The Rover GUI 4 | 5 | 6 | resources/swarmathon-gui-icon.png 7 | Rover GUI 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/resources/images.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/src/rqt_rover_gui/resources/images.jpeg -------------------------------------------------------------------------------- /src/rqt_rover_gui/resources/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | rover.png 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/resources/rover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/src/rqt_rover_gui/resources/rover.png -------------------------------------------------------------------------------- /src/rqt_rover_gui/resources/swarmathon-gui-icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BCLab-UNM/SwarmBaseCode-ROS/013bc3d1a396cf373de681a050ef0cad53bc4934/src/rqt_rover_gui/resources/swarmathon-gui-icon.png -------------------------------------------------------------------------------- /src/rqt_rover_gui/scripts/rqt_rover_gui: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_rover_gui/RoverGUI')) 9 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_rover_gui'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/BWTabWidget.cpp: -------------------------------------------------------------------------------- 1 | #include "BWTabWidget.h" 2 | 3 | #include 4 | #include 5 | 6 | /*! 7 | * Constructor. This function sets the color of the tabs at the top of the GUI. 8 | * The stylesheets method does not allow changing the color of tab bars. 9 | */ 10 | BWTabWidget::BWTabWidget(QWidget *p) : QTabWidget(p) { 11 | QPalette pal = tabBar()->palette(); 12 | 13 | pal.setColor(QPalette::WindowText, QColor(255,255,255)); 14 | pal.setColor(QPalette::Window, QColor(0,0,0)); 15 | pal.setColor(QPalette::Button, QColor(0,0,0)); 16 | pal.setColor(QPalette::ButtonText, QColor(0,0,0)); 17 | pal.setColor(QPalette::Base, QColor(0, 0, 0)); 18 | pal.setColor(QPalette::AlternateBase, QColor(0, 0, 0)); 19 | pal.setColor(QPalette::Text, Qt::black); 20 | 21 | tabBar()->setPalette(pal); 22 | } 23 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/BWTabWidget.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \brief This subclass just provides a way to change the color of the 3 | * QTabWidget in the GUI. Changing colors isn't possible just using 4 | * the UI editor. 5 | * \author Matthew Fricke 6 | * \date November 11th 2015 7 | * \todo Code works properly. 8 | * \class BQTabWidget 9 | */ 10 | 11 | #ifndef BWTABWIDGET_H 12 | #define BWTABWIDGET_H 13 | 14 | #include 15 | 16 | class BWTabWidget : public QTabWidget { 17 | public: 18 | BWTabWidget(QWidget *p=0); 19 | }; 20 | 21 | #endif // BWTABWIDGET_H 22 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/GPSFrame.cpp: -------------------------------------------------------------------------------- 1 | #ifndef rqt_rover_gui_GPSFrame 2 | #define rqt_rover_gui_GPSFrame 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace rqt_rover_gui 9 | { 10 | 11 | GPSFrame::GPSFrame(QWidget *parent, Qt::WindowFlags flags) : QFrame(parent) { 12 | connect(this, SIGNAL(delayedUpdate()), this, SLOT(update()), 13 | Qt::QueuedConnection); 14 | frames = 0; 15 | } 16 | 17 | void GPSFrame::paintEvent(QPaintEvent* event) { 18 | QPainter painter(this); 19 | painter.setPen(Qt::white); 20 | 21 | // Track the frames per second for development purposes 22 | float fps = (float)frames / ((float)frame_rate_timer.elapsed() / 1000.0); 23 | QString frames_per_second = QString::number(fps, 'f', 0) + " FPS"; 24 | 25 | QFontMetrics fm(painter.font()); 26 | painter.drawText(this->width()-fm.width(frames_per_second), fm.height(), 27 | frames_per_second); 28 | 29 | frames++; 30 | 31 | // time how long it takes to dispay 100 frames 32 | if (!(frames % 100)) { 33 | frame_rate_timer.start(); 34 | frames = 0; 35 | } 36 | // end frames per second 37 | 38 | painter.drawText(QPoint(50,50), "GPS Frame"); 39 | painter.setPen(Qt::white); 40 | } 41 | 42 | } /* END: namespace rqt_rover_gui */ 43 | 44 | #endif /* rqt_rover_gui_GPSFrame */ 45 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/GPSFrame.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * \brief This frame is intended to show information about the quality of 3 | * the GPS sensor data, for example, the number of satellites current 4 | * sending data. 5 | * \author Matthew Fricke 6 | * \date November 11th 2015 7 | * \todo This frame is not currently being used and is currently just a place 8 | * holder class. 9 | * \class GPSFrame 10 | */ 11 | 12 | #ifndef GPSFRAME_H 13 | #define GPSFRAME_H 14 | 15 | #include // for frame rate 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include // for STL pair 22 | 23 | using namespace std; 24 | 25 | namespace rqt_rover_gui 26 | { 27 | class GPSFrame : public QFrame 28 | { 29 | Q_OBJECT 30 | 31 | public: 32 | GPSFrame(QWidget *parent, Qt::WindowFlags = 0); 33 | 34 | signals: 35 | void delayedUpdate(); 36 | 37 | public slots: 38 | // currently, no class-defined slots are being used 39 | 40 | protected: 41 | void paintEvent(QPaintEvent *event); 42 | 43 | private: 44 | QTime frame_rate_timer; 45 | int frames; 46 | }; 47 | } 48 | 49 | #endif // GPSFrame_H 50 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/Version.cpp.in: -------------------------------------------------------------------------------- 1 | // This file is used by cmake to generate a .cpp file that contains the build 2 | // time and git version. The values are set in CMakeLists.txt using the 3 | // configure_file command. This method should keep the git version display 4 | // current in the GUI with minimal recompilation. 5 | 6 | #include "Version.h" 7 | 8 | const QString GIT_BRANCH = "@GIT_BRANCH@"; 9 | const QString GIT_VERSION = "@GIT_VERSION@"; 10 | const QString BUILD_TIME = "@BUILD_TIME@"; 11 | -------------------------------------------------------------------------------- /src/rqt_rover_gui/src/Version.h: -------------------------------------------------------------------------------- 1 | // This header exports the git version and build time for display by the GUI 2 | 3 | #ifndef VERSION_H 4 | #define VERSION_H 5 | 6 | #include 7 | 8 | extern const QString GIT_BRANCH; 9 | extern const QString GIT_VERSION; 10 | extern const QString BUILD_TIME; 11 | 12 | #endif //VERSION_H 13 | -------------------------------------------------------------------------------- /src/sbridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sbridge) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | std_msgs 8 | swarmie_msgs 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS geometry_msgs roscpp std_msgs swarmie_msgs 13 | ) 14 | 15 | include_directories( 16 | ${catkin_INCLUDE_DIRS} include 17 | ) 18 | 19 | add_executable( 20 | sbridge src/main.cpp src/sbridge.cpp 21 | ) 22 | 23 | target_link_libraries( 24 | sbridge 25 | ${catkin_LIBRARIES} 26 | ) 27 | 28 | -------------------------------------------------------------------------------- /src/sbridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sbridge 4 | 0.0.1 5 | Gazebo interface to provide formatted drive control 6 | 7 | NASA Swarmathon 8 | 9 | MIT 10 | 11 | catkin 12 | 13 | geometry_msgs 14 | roscpp 15 | 16 | std_msgs 17 | 18 | 19 | swarmie_msgs 20 | 21 | geometry_msgs 22 | roscpp 23 | 24 | std_msgs 25 | 26 | 27 | swarmie_msgs 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/sbridge/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "sbridge.h" 3 | 4 | using namespace std; 5 | 6 | void sigintEventHandler(int signal); 7 | 8 | int main(int argc, char **argv) 9 | { 10 | sleep(10); 11 | 12 | char host[128]; 13 | gethostname(host, sizeof (host)); 14 | string hostname(host); 15 | string published_name; 16 | 17 | if (argc >= 2) 18 | { 19 | published_name = argv[1]; 20 | cout << "Welcome to the world of tomorrow " << published_name << "! ABridge module started." << endl; 21 | } 22 | else 23 | { 24 | published_name = hostname; 25 | cout << "No Name Selected. Default is: " << published_name << endl; 26 | } 27 | 28 | ros::init(argc, argv, (hostname + "_SBRIDGE"), ros::init_options::NoSigintHandler); 29 | 30 | sbridge sb(published_name); 31 | 32 | signal(SIGINT, sigintEventHandler); 33 | 34 | ros::spin(); 35 | 36 | return EXIT_SUCCESS; 37 | } 38 | 39 | void sigintEventHandler(int signal) 40 | { 41 | ros::shutdown(); 42 | } 43 | 44 | -------------------------------------------------------------------------------- /src/sbridge/src/sbridge.h: -------------------------------------------------------------------------------- 1 | #ifndef SBRIDGE 2 | #define SBRIDGE 3 | 4 | // ROS messages 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | using namespace std; 16 | 17 | /** 18 | * This class translates drive controls into Gazebo 19 | * friendly velocities. 20 | */ 21 | class sbridge { 22 | 23 | public: 24 | 25 | sbridge(std::string published_name); 26 | void cmdHandler(const swarmie_msgs::Skid::ConstPtr& message); 27 | ~sbridge(); 28 | 29 | private: 30 | 31 | // Publishers 32 | ros::Publisher skidsteer_publisher; 33 | ros::Publisher heartbeat_publisher; 34 | ros::Publisher info_log_publisher; 35 | 36 | // Subscribers 37 | ros::Subscriber drive_control_subscriber; 38 | ros::Subscriber mode_subscriber; 39 | 40 | // Timer callback handler 41 | void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event); 42 | 43 | ros::Timer publish_heartbeat_timer; 44 | 45 | geometry_msgs::Twist velocity; 46 | }; 47 | 48 | #endif /* SBRIDGE */ 49 | -------------------------------------------------------------------------------- /src/swarmie_msgs/msg/Recruitment.msg: -------------------------------------------------------------------------------- 1 | # Messages used for recruitment 2 | float32 x 3 | float32 y 4 | std_msgs/String name 5 | -------------------------------------------------------------------------------- /src/swarmie_msgs/msg/Skid.msg: -------------------------------------------------------------------------------- 1 | float32 left 2 | float32 right 3 | -------------------------------------------------------------------------------- /src/swarmie_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | # Defines a labeled point 2 | uint32 ACTION_ADD=0 3 | uint32 ACTION_REMOVE=1 4 | uint32 ACTION_REACHED=2 5 | uint32 action 6 | uint32 id 7 | float32 x 8 | float32 y --------------------------------------------------------------------------------