#include "ros/ros.h"#include "orientation.h"#include <stdio.h>#include "corobot_msgs/PosMsg.h"#include "corobot_msgs/PowerMsg.h"#include "corobot_msgs/SensorMsg.h"#include "sensor_msgs/MagneticField.h"#include "sensor_msgs/Imu.h"#include "std_msgs/Int16.h"#include <phidget21.h>#include <tf/transform_datatypes.h>#include <diagnostic_updater/diagnostic_updater.h>#include <diagnostic_updater/publisher.h>#include <corobot_diagnostics/diagnostics.h>#include <std_srvs/Empty.h>
Go to the source code of this file.
Functions | |
| int | AnalogInputHandler (CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int Value) |
| bool | calibrate_gyroscope (std_srvs::Empty::Request &request, std_srvs::Empty::Response &response) |
| int | DigitalInputHandler (CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int State) |
| callback that will run if an input changes. Index - Index of the input that generated the event, State - boolean (0 or 1) representing the input state (on or off) | |
| int | ErrorHandler (CPhidgetHandle IFK, void *userptr, int ErrorCode, const char *unknown) |
| int | interfacekit_simple () |
| static float | irVoltageToDistance (float volts) |
| Description: Convert a voltage from the Phidgets 8/8/8 board into a distance, for the Infrared sensors return 25.4 meters as a distance if the value is out of range otherwise returns a valid distance in meters. | |
| int | main (int argc, char *argv[]) |
| void | phidget_ik_diagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat) |
| void | phidget_spatial_diagnostic (diagnostic_updater::DiagnosticStatusWrapper &stat) |
| int | sendSonarResult () |
| Function that will use use the sonar to later acquire their value. | |
| static double | sonarVoltageToMeters (int value) |
| int | SpatialDataHandler (CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count) |
Variables | |
| int | batteryPort = 0 |
| ros::Publisher | bumper_pub |
| int | bwOutput = -1 |
| ros::ServiceServer | calibrate_gyroscope_service |
| int | firstSonarInput = -1 |
| CPhidgetInterfaceKitHandle | ifKit = 0 |
| bool | imu = true |
| ros::Publisher | imu_pub |
| int | interfaceKitError = 0 |
| int | irBackPort = 2 |
| ros::Publisher | irData_pub |
| int | irFrontPort = 1 |
| int | lastSonarInput = -1 |
| bool | m_rearBumperPresent = false |
| ros::Publisher | mag_pub |
| Orientation | orientation_calculation |
| ros::Publisher | other_pub |
| ros::Publisher | powerdata_pub |
| ros::Publisher | sonar_pub |
| bool | sonarsPresent = false |
| CPhidgetSpatialHandle | spatial = 0 |
| int | spatialError = 0 |
| int | strobeOutput = -1 |
| int AnalogInputHandler | ( | CPhidgetInterfaceKitHandle | IFK, |
| void * | usrptr, | ||
| int | Index, | ||
| int | Value | ||
| ) |
Definition at line 183 of file corobot_phidget.cpp.
| bool calibrate_gyroscope | ( | std_srvs::Empty::Request & | request, |
| std_srvs::Empty::Response & | response | ||
| ) |
Definition at line 291 of file corobot_phidget.cpp.
| int DigitalInputHandler | ( | CPhidgetInterfaceKitHandle | IFK, |
| void * | usrptr, | ||
| int | Index, | ||
| int | State | ||
| ) |
callback that will run if an input changes. Index - Index of the input that generated the event, State - boolean (0 or 1) representing the input state (on or off)
Definition at line 151 of file corobot_phidget.cpp.
| int ErrorHandler | ( | CPhidgetHandle | IFK, |
| void * | userptr, | ||
| int | ErrorCode, | ||
| const char * | unknown | ||
| ) |
Definition at line 89 of file corobot_phidget.cpp.
| int interfacekit_simple | ( | ) |
Definition at line 297 of file corobot_phidget.cpp.
| static float irVoltageToDistance | ( | float | volts | ) | [static] |
Description: Convert a voltage from the Phidgets 8/8/8 board into a distance, for the Infrared sensors return 25.4 meters as a distance if the value is out of range otherwise returns a valid distance in meters.
Definition at line 102 of file corobot_phidget.cpp.
| int main | ( | int | argc, |
| char * | argv[] | ||
| ) |
Definition at line 413 of file corobot_phidget.cpp.
| void phidget_ik_diagnostic | ( | diagnostic_updater::DiagnosticStatusWrapper & | stat | ) |
Function that will report the status of the hardware to the diagnostic topic
Definition at line 385 of file corobot_phidget.cpp.
Function that will report the status of the hardware to the diagnostic topic
Definition at line 399 of file corobot_phidget.cpp.
| int sendSonarResult | ( | ) |
Function that will use use the sonar to later acquire their value.
Definition at line 138 of file corobot_phidget.cpp.
| static double sonarVoltageToMeters | ( | int | value | ) | [static] |
Voltage to distance function that is used for the sonar sensors
| value | value from 0 to 1000 representing a voltage value from 0 to vcc (5V) |
Definition at line 125 of file corobot_phidget.cpp.
| int SpatialDataHandler | ( | CPhidgetSpatialHandle | spatial, |
| void * | userptr, | ||
| CPhidgetSpatial_SpatialEventDataHandle * | data, | ||
| int | count | ||
| ) |
Receive new IMU data, calculate the new orientation and publish data on the topic
Definition at line 247 of file corobot_phidget.cpp.
| int batteryPort = 0 |
Definition at line 79 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
| int bwOutput = -1 |
Definition at line 69 of file corobot_phidget.cpp.
Definition at line 66 of file corobot_phidget.cpp.
| int firstSonarInput = -1 |
Definition at line 78 of file corobot_phidget.cpp.
| CPhidgetInterfaceKitHandle ifKit = 0 |
Definition at line 47 of file corobot_phidget.cpp.
| bool imu = true |
Definition at line 59 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
| int interfaceKitError = 0 |
Definition at line 84 of file corobot_phidget.cpp.
| int irBackPort = 2 |
Definition at line 81 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
| int irFrontPort = 1 |
Definition at line 80 of file corobot_phidget.cpp.
| int lastSonarInput = -1 |
Definition at line 75 of file corobot_phidget.cpp.
| bool m_rearBumperPresent = false |
Definition at line 53 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
Definition at line 50 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
Definition at line 62 of file corobot_phidget.cpp.
| bool sonarsPresent = false |
Definition at line 56 of file corobot_phidget.cpp.
| CPhidgetSpatialHandle spatial = 0 |
Definition at line 48 of file corobot_phidget.cpp.
| int spatialError = 0 |
Definition at line 84 of file corobot_phidget.cpp.
| int strobeOutput = -1 |
Definition at line 72 of file corobot_phidget.cpp.