diff --git a/config/minimal_epuck2.rviz b/config/minimal_epuck2.rviz new file mode 100644 index 0000000..ccf5e48 --- /dev/null +++ b/config/minimal_epuck2.rviz @@ -0,0 +1,361 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1/Status1 + - /Odometry1/Covariance1/Position1 + - /Odometry1/Covariance1/Orientation1 + - /Imu1/Status1 + Splitter Ratio: 0.639118016 + Tree Height: 336 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan + - Class: rviz_plugin_tutorials/Teleop + Name: Teleop + Topic: mobile_base/cmd_vel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.0500000007 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox0 + Queue Size: 100 + Topic: /proximity0 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox1 + Queue Size: 100 + Topic: /proximity1 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox2 + Queue Size: 100 + Topic: /proximity2 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox3 + Queue Size: 100 + Topic: /proximity3 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox4 + Queue Size: 100 + Topic: /proximity4 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox5 + Queue Size: 100 + Topic: /proximity5 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox6 + Queue Size: 100 + Topic: /proximity6 + Unreliable: false + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 255; 255 + Enabled: true + Name: Prox7 + Queue Size: 100 + Topic: /proximity7 + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + body_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: epuck_robot_0 + Update Interval: 0 + Value: true + Visual Enabled: true + - Angle Tolerance: 0.0500000007 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.100000001 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Color: 255; 25; 0 + Head Length: 0.00499999989 + Head Radius: 0.00200000009 + Shaft Length: 0.00700000022 + Shaft Radius: 0.00100000005 + Value: Arrow + Topic: /odom + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /floor + Name: Floor + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /motor_speed + Name: Speed + Namespaces: + {} + Queue Size: 1 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /microphone + Name: Microphone + Namespaces: + "": true + Queue Size: 1 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensities + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 1 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 72 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 3 + Size (m): 0.00499999989 + Style: Spheres + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Acceleration properties: + Acc. vector alpha: 1 + Acc. vector color: 255; 0; 0 + Acc. vector scale: 1 + Derotate acceleration: true + Enable acceleration: false + Axes properties: + Axes scale: 1 + Enable axes: true + Box properties: + Box alpha: 1 + Box color: 255; 0; 0 + Enable box: false + x_scale: 1 + y_scale: 1 + z_scale: 1 + Class: rviz_imu_plugin/Imu + Enabled: true + Name: Imu + Topic: /imu + Unreliable: false + Value: true + fixed_frame_orientation: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 0.418160737 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.655981719 + Y: 0.0647886693 + Z: -1.78813934e-07 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.525203586 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 1.20539534 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 772 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000017d0000027afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001df000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070010000020d000000950000004900ffffff00000001000001590000027afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000027a000000ad00fffffffb0000000a0049006d006100670065010000015b000001470000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000031d0000027a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Teleop: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1535 + X: 75 + Y: 34 diff --git a/config/single_epuck_driver_rviz.rviz b/config/single_epuck_driver_rviz.rviz index 044bd8d..d176479 100644 --- a/config/single_epuck_driver_rviz.rviz +++ b/config/single_epuck_driver_rviz.rviz @@ -3,8 +3,11 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.639118 + Expanded: + - /Odometry1/Covariance1/Position1 + - /Odometry1/Covariance1/Orientation1 + - /Image1/Status1 + Splitter Ratio: 0.639118016 Tree Height: 336 - Class: rviz/Selection Name: Selection @@ -14,7 +17,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 @@ -32,12 +35,12 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 0.05 + Cell Size: 0.0500000007 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 @@ -57,6 +60,7 @@ Visualization Manager: Name: Prox0 Queue Size: 100 Topic: /proximity0 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -66,6 +70,7 @@ Visualization Manager: Name: Prox1 Queue Size: 100 Topic: /proximity1 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -75,6 +80,7 @@ Visualization Manager: Name: Prox2 Queue Size: 100 Topic: /proximity2 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -84,6 +90,7 @@ Visualization Manager: Name: Prox3 Queue Size: 100 Topic: /proximity3 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -93,6 +100,7 @@ Visualization Manager: Name: Prox4 Queue Size: 100 Topic: /proximity4 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -102,6 +110,7 @@ Visualization Manager: Name: Prox5 Queue Size: 100 Topic: /proximity5 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -111,6 +120,7 @@ Visualization Manager: Name: Prox6 Queue Size: 100 Topic: /proximity6 + Unreliable: false Value: true - Alpha: 0.5 Buffer Length: 1 @@ -120,6 +130,7 @@ Visualization Manager: Name: Prox7 Queue Size: 100 Topic: /proximity7 + Unreliable: false Value: true - Class: rviz/TF Enabled: false @@ -171,36 +182,39 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Angle Tolerance: 0.01 + - Angle Tolerance: 0.0500000007 Class: rviz/Odometry - Color: 255; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false Enabled: true Keep: 100 - Length: 0.01 Name: Odometry - Position Tolerance: 0.01 + Position Tolerance: 0.100000001 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Color: 255; 25; 0 + Head Length: 0.00499999989 + Head Radius: 0.00200000009 + Shaft Length: 0.00700000022 + Shaft Radius: 0.00100000005 + Value: Arrow Topic: /odom - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.01 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.1 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.1 - y_scale: 0.1 - z_scale: 0.1 - Class: rviz_imu_plugin/Imu - Enabled: true - Name: Imu - Topic: /accel + Unreliable: false Value: true - Class: rviz/Marker Enabled: true @@ -215,7 +229,7 @@ Visualization Manager: Marker Topic: /motor_speed Name: Speed Namespaces: - "": true + {} Queue Size: 1 Value: true - Class: rviz/Marker @@ -223,19 +237,20 @@ Visualization Manager: Marker Topic: /microphone Name: Microphone Namespaces: - "": true + {} Queue Size: 1 Value: true - Class: rviz/Image Enabled: true - Image Topic: /camera + Image Topic: /raspicam_node/image Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed + Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -260,9 +275,10 @@ Visualization Manager: Queue Size: 10 Selectable: false Size (Pixels): 3 - Size (m): 0.005 + Size (m): 0.00499999989 Style: Spheres Topic: /scan + Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true @@ -273,10 +289,22 @@ Visualization Manager: Enabled: true Name: Map Topic: /map + Unreliable: false + Use Timestamp: false Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: false + History Length: 1 + Name: Imu + Topic: /imu + Unreliable: false + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: map Frame Rate: 30 Name: root @@ -298,22 +326,25 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 0.945416 + Distance: 10 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.00113173 - Y: 0.323188 - Z: 2.33572e-07 + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.689797 + Near Clip Distance: 0.00999999978 + Pitch: 0.785398185 Target Frame: base_link Value: XYOrbit (rviz) - Yaw: 1.5704 + Yaw: 0.785398185 Saved: ~ Window Geometry: Displays: @@ -323,7 +354,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000017d0000027afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001df000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070010000020d000000950000004900ffffff000000010000012c0000027afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001a4000000b000fffffffb0000000a0049006d00610067006501000001d2000000d00000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000034a0000027a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000017d0000027afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001df000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070010000020d000000950000004900ffffff00000001000001590000027afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000012d000000ad00fffffffb0000000a0049006d006100670065010000015b000001470000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000031d0000027a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Teleop: diff --git a/launch/epuck_controller.launch b/launch/epuck_controller.launch index 04f02ab..abca10f 100644 --- a/launch/epuck_controller.launch +++ b/launch/epuck_controller.launch @@ -1,7 +1,6 @@ - @@ -16,43 +15,26 @@ - - + - - - - - - - - - - + + - - - - - + + + - - - - - - - + @@ -61,7 +43,7 @@ - + @@ -152,4 +134,18 @@ + + + + + + + + + + + + + + diff --git a/launch/epuck_minimal.launch b/launch/epuck_minimal.launch new file mode 100644 index 0000000..58b3033 --- /dev/null +++ b/launch/epuck_minimal.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/epuck_driver_cpp.cpp b/src/epuck_driver_cpp.cpp index 4539576..68272ee 100644 --- a/src/epuck_driver_cpp.cpp +++ b/src/epuck_driver_cpp.cpp @@ -3,10 +3,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -20,6 +16,18 @@ #include #include #include +extern "C" { + #include + #include + #include + #include + #include + #include /* for I2C_SLAVE */ + //#include + #include + #include + #include +} #define DEBUG_CONNECTION_INIT 1 #define DEBUG_ROS_PARAMS 1 @@ -27,32 +35,34 @@ #define DEBUG_UPDATE_SENSORS_DATA 0 #define DEBUG_COMMUNICATION_ERROR 1 #define DEBUG_ODOMETRY 0 -#define DEBUG_ACCELEROMETER 0 +#define DEBUG_IMU 0 #define DEBUG_SPEED_RECEIVED 0 #define DEBUG_LED_RECEIVED 0 -#define DEBUG_CAMERA_INIT 0 + +#define I2C_CHANNEL "/dev/i2c-12" +#define LEGACY_I2C_CHANNEL "/dev/i2c-4" #define READ_TIMEOUT_SEC 10 // 10 seconds, keep it high to avoid desynchronize when there are communication delays due to Bluetooth. #define READ_TIMEOUT_USEC 0 #define MAX_CONSECUTIVE_TIMEOUT 3 -#define SENSORS_NUM 7 -#define ACCELEROMETER 0 +#define SENSORS_NUM 10 +#define IMU 0 #define MOTOR_SPEED 1 #define FLOOR 2 #define PROXIMITY 3 #define MOTOR_POSITION 4 #define MICROPHONE 5 #define CAMERA 6 +#define TV_REMOTE 7 +#define SELECTOR 8 +#define TIME_OF_FLIGHT 9 -#define ACTUATORS_NUM 3 +#define ACTUATORS_NUM 4 #define MOTORS 0 #define LEDS 1 #define MOTORS_POS 2 - -#define CAM_MAX_BUFFER_SIZE 3203 -#define CAM_MODE_GRAY 0 -#define CAM_MODE_RGB 1 +#define SPEAKER 3 #define WHEEL_DIAMETER 4 // cm. #define WHEEL_SEPARATION 5.3 // Separation between wheels (cm). @@ -61,35 +71,50 @@ #define MOT_STEP_DIST (WHEEL_CIRCUMFERENCE/1000.0) // Distance for each motor step (meters); a complete turn is 1000 steps (0.000125 meters per step (m/steps)). #define ROBOT_RADIUS 0.035 // meters. -#define LED_NUMBER 10 // total number of LEDs on the robot (0-7=leds, 8=body, 9=front) +#define LEDS_NUM 4 // Number of LEDs on the robot. +#define RGB_LEDS_NUM 4 // Number of RGB LEDs on the robot. + +#define ACTUATORS_SIZE (19+1) // Data + checksum. +#define SENSORS_SIZE (46+1) // Data + checksum. +#define ROBOT_ADDR 0x1F + +#define NUM_SAMPLES_CALIBRATION 20 +#define AK8963_ADDRESS 0x0C // Address of magnetometer +#define AK8963_XOUT_L 0x03 // data -char pcToRobotBuff[10]; -char robotToPcBuff[255]; +#define MPU9250_ADDRESS_AD1_0 0x68 // Device address when AD1 = 0 +#define MPU9250_ADDRESS_AD1_1 0x69 + +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define TEMP_OUT_H 0x41 +#define GYRO_XOUT_H 0x43 + +#define GRAVITY_MPU9250 16384 // 1 g for 16 bits in 2g scale mode +#define DEG2RAD(deg) (deg / 180 * M_PI) +#define STANDARD_GRAVITY 9.80665f +#define ACC_RAW2G (2.0/32768.0f) //2G scale for int16 raw value +#define GYRO_RAW2DPS (250.0/32768.0f) //250DPS (degrees per second) scale for int16 raw value + +int fh; +char zero_to_epuck_buff[ACTUATORS_SIZE]; +char epuck_to_zero_buff[SENSORS_SIZE]; bool enabledSensors[SENSORS_NUM]; bool changedActuators[ACTUATORS_NUM]; int speedLeft = 0, speedRight = 0; -unsigned char ledState[LED_NUMBER] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; +unsigned char ledState[LEDS_NUM] = {0, 0, 0, 0}; +unsigned char rgbLedState[RGB_LEDS_NUM][3]; int stepsLeft = 0, stepsRight = 0; -int rfcommSock; -int sock; -int devId; -unsigned int bytesToReceive; -unsigned int bytesToSend; std::string epuckname; struct timeval currentTime2, lastTime2; struct timeval currentTime3, lastTime3; int consecutiveReadTimeout = 0; -int camWidth, camHeight, camZoom, camMode, camXoffset, camYoffset; -int accData[3]; -int motorSpeedData[2]; -int floorData[5]; int proxData[8]; int motorPositionData[2]; -int micData[3]; -unsigned char *camData; -bool newImageReceived = false; -int imageSize = 0; +int micData[4]; +uint8_t selectorData; +uint8_t tvRemoteData; ros::Publisher proxPublisher[8]; sensor_msgs::Range proxMsg[8]; @@ -97,15 +122,12 @@ ros::Publisher laserPublisher; sensor_msgs::LaserScan laserMsg; ros::Publisher odomPublisher; nav_msgs::Odometry odomMsg; -ros::Publisher imagePublisher; -ros::Publisher accelPublisher; -sensor_msgs::Imu accelMsg; -ros::Publisher motorSpeedPublisher; -visualization_msgs::Marker motorSpeedMsg; ros::Publisher microphonePublisher; visualization_msgs::Marker microphoneMsg; +ros::Publisher imuPublisher; +sensor_msgs::Imu imuMsg; +ros::Publisher motorSpeedPublisher; ros::Publisher floorPublisher; -visualization_msgs::Marker floorMsg; ros::Subscriber cmdVelSubscriber, cmdLedSubscriber; @@ -118,412 +140,291 @@ double deltaSteps, deltaTheta; ros::Time currentTime, lastTime, currentTimeMap, lastTimeMap; int overflowCountLeft = 0, overflowCountRight = 0; -void clearCommunicationBuffer() { - - char buffer[64]; - struct timeval timeout; - fd_set readfds; - int retval; - int trials = 0; - - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "sending enter..." << std::endl; - buffer[0] = '\r'; - FD_ZERO(&readfds); - FD_SET(rfcommSock, &readfds); - write(rfcommSock, buffer, 1); - while(1) { - timeout.tv_sec=0; // The timeout need to be set every time because the "select" may modify it. - timeout.tv_usec=500000; - retval = select(rfcommSock+1,&readfds,NULL,NULL,&timeout); - if (retval!=0) { - int n = read(rfcommSock, buffer, 64); - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "read " << n << " bytes" << std::endl; - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "content: " << buffer << std::endl; - memset(buffer, 0x0, 64); - } else { - break; - } - } - - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "requesting version..." << std::endl; - buffer[0] = 'V'; - buffer[1] = '\r'; - FD_ZERO(&readfds); - FD_SET(rfcommSock, &readfds); - write(rfcommSock, buffer, 2); - trials = 0; - while(1) { - timeout.tv_sec=0; // The timeout need to be set every time because the "select" may modify it. - timeout.tv_usec=500000; - retval = select(rfcommSock+1, &readfds, NULL, NULL, &timeout); - if (retval!=0) { - int n = read(rfcommSock, buffer, 64); - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "read " << n << " bytes" << std::endl; - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "content: " << buffer << std::endl; - memset(buffer, 0x0, 64); - } else { - trials++; - if(trials >= 1) { - break; - } - } - } - -} +uint8_t imu_addr = MPU9250_ADDRESS_AD1_0; +uint8_t accData[6]; +uint8_t gyroData[6]; +uint8_t temperatureData; +int16_t accValue[3]; +int32_t accSum[3] = {0, 0, 0}; +int16_t accOffset[3] = {0, 0, 0}; +int16_t gyroValue[3]; +int32_t gyroSum[3] = {0, 0, 0}; +int16_t gyroOffset[3] = {0, 0, 0}; -int initConnectionWithRobotId(int robotId) { - std::stringstream ss; - - // open device - devId = hci_get_route(NULL); - if (devId < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't get bluetooth adapter ID"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - return -1; - } - - // open socket - sock = hci_open_dev(devId); - if (sock < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't open bluetooth adapter"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - return -1; - } +bool debug_enabled = false; +uint8_t debug_count = 0; - int trials = 3; // Try looking for the robot 3 times before giving up. - while(trials) { - // query - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "Scanning bluetooth:" << std::endl; - //int length = 8; /* ~10 seconds */ - int length = 4; /* ~5 seconds */ - inquiry_info *info = NULL; - int devicesCount = 0; - while(1) { - // device id, query length (last 1.28 * length seconds), max devices, lap ??, returned array, flag - devicesCount = hci_inquiry(devId, length, 255, NULL, &info, IREQ_CACHE_FLUSH); - if (devicesCount < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't query bluetooth"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - if(errno!=EBUSY) { // EBUSY means the Bluetooth device is currently used by another process (this happens when - close(sock); // we want to connect to multiple robots simultaneously); in this case wait a little and retry. - return -1; // All others errors are treated normally. - } else { - usleep(1000000); - } - } else { - break; - } - } - - bool found = false; - for (int i = 0; i < devicesCount; i++) { - char addrString[19]; - char addrFriendlyName[256]; - ba2str(&(info+i)->bdaddr, addrString); - if (hci_read_remote_name(sock, &(info+i)->bdaddr, 256, addrFriendlyName, 0) < 0) { - strcpy(addrFriendlyName, "[unknown]"); - } - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "\t" << addrString << " " << addrFriendlyName << std::endl; - if (strncmp("e-puck_", addrFriendlyName, 7) == 0) { - int id; - if (sscanf(addrFriendlyName + 7, "%d", &id) && (id == robotId)) { - if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "Contacting e-puck " << id << std::endl; - - // set the connection parameters (who to connect to) - struct sockaddr_rc addr; - addr.rc_family = AF_BLUETOOTH; - addr.rc_channel = (uint8_t) 1; - addr.rc_bdaddr = (info+i)->bdaddr; - - // allocate a socket - rfcommSock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); - - // connect to server - int status = ::connect(rfcommSock, (struct sockaddr *)&addr, sizeof(addr)); - - if (status == 0) { - clearCommunicationBuffer(); - found = true; - } else { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't connect to rfcomm socket"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - return -1; - } - break; - } - } - } - if(found) { - if(hci_close_dev(sock) < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Can't close HCI device"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - } - return 0; - } else { - trials--; - } - } - if(hci_close_dev(sock) < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Can't close HCI device"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - } - return -1; - -} +int initConnectionWithRobot(void) { -int initConnectionWithRobotAddress(const char *address) { - - std::stringstream ss; - struct sockaddr_rc addr; // set the connection parameters (who to connect to) - addr.rc_family = AF_BLUETOOTH; - addr.rc_channel = (uint8_t) 1; - str2ba(address, &addr.rc_bdaddr); - - // allocate a socket - rfcommSock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); - - // connect to server - int status = ::connect(rfcommSock, (struct sockaddr *)&addr, sizeof(addr)); - - if (status == 0) { - clearCommunicationBuffer(); - return 0; - } else { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't connect to rfcomm socket"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - return -1; - } + // Set the I2C timeout to 20 ms (instead of 1 second). This need to be done on the "swticher" bus channel. + int fh1 = open("/dev/i2c-1", O_RDWR); + if(ioctl(fh1, I2C_TIMEOUT, 2) < 0) { + perror("fail to set i2c1 timeout"); + } + close(fh1); + fh = open(I2C_CHANNEL, O_RDWR); + if(fh < 0) { // Try with bus number used in older kernel + fh = open(LEGACY_I2C_CHANNEL, O_RDWR); + if(fh < 0) { + perror("Cannot open I2C device"); + return -1; + } + } + return 0; } void closeConnection() { - std::stringstream ss; - - if(close(rfcommSock) < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Can't close rfcomm socket"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - } - -// if(shutdown(rfcommSock, SHUT_RDWR) < 0) { -// ss.str(""); -// ss << "[" << epuckname << "] " << "Can't shutdown rfcomm socket"; -// if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); -// } else { -// if(DEBUG_CONNECTION_INIT)std::cout << "[" << epuckname << "] " << "rfcomm shutdown correctly" << std::endl; -// } + close(fh); } void updateActuators() { char buff[36]; // 6 (initial size) + 3*LED_NUM (=10) - + if(changedActuators[MOTORS]) { changedActuators[MOTORS] = false; - buff[0] = -'D'; - buff[1] = speedLeft&0xFF; - buff[2] = (speedLeft>>8)&0xFF; - buff[3] = speedRight&0xFF; - buff[4] = (speedRight>>8)&0xFF; - buff[5] = 0; - write(rfcommSock, buff, 6); + zero_to_epuck_buff[0] = speedLeft&0xFF; + zero_to_epuck_buff[1] = speedLeft>>8; + zero_to_epuck_buff[2] = speedRight&0xFF; + zero_to_epuck_buff[3] = speedRight>>8; } if(changedActuators[LEDS]) { + changedActuators[LEDS] = false; unsigned char i, pos = 0; - - //[-L][LED_NUM][STATE] set led state: led number (0-7=leds, 8=body, 9=front), state (0=0ff, 1=on, 2=inverse) - changedActuators[LEDS] = false; - for (i = 0; i < LED_NUMBER; i++) { - buff[pos++] = -'L'; - buff[pos++] = i; - buff[pos++] = ledState[i]; - } - buff[pos] = 0; - write(rfcommSock, buff, pos + 1); - } - - if(changedActuators[MOTORS_POS]) { - changedActuators[MOTORS_POS] = false; - buff[0] = -'P'; - buff[1] = stepsLeft&0xFF; - buff[2] = (stepsLeft>>8)&0xFF; - buff[3] = stepsRight&0xFF; - buff[4] = (stepsRight>>8)&0xFF; - buff[5] = 0; - write(rfcommSock, buff, 6); + if(ledState[0]) { + zero_to_epuck_buff[5] |= 1; + } else { + zero_to_epuck_buff[5] &= ~(1); + } + if(ledState[1]) { + zero_to_epuck_buff[5] |= 2; + } else { + zero_to_epuck_buff[5] &= ~(2); + } + if(ledState[2]) { + zero_to_epuck_buff[5] |= 4; + } else { + zero_to_epuck_buff[5] &= ~(4); + } + if(ledState[3]) { + zero_to_epuck_buff[5] |= 8; + } else { + zero_to_epuck_buff[5] &= ~(8); + } + //zero_to_epuck_buff[5] = 0x0; // LED1, LED3, LED5, LED7 on/off flag + zero_to_epuck_buff[6] = 0; // LED2 red + zero_to_epuck_buff[7] = 0; // LED2 green + zero_to_epuck_buff[8] = 0; // LED2 blue + zero_to_epuck_buff[9] = 0; // LED4 red + zero_to_epuck_buff[10] = 0; // LED4 green + zero_to_epuck_buff[11] = 0; // LED4 blue + zero_to_epuck_buff[12] = 0; // LED6 red + zero_to_epuck_buff[13] = 0; // LED6 green + zero_to_epuck_buff[14] = 0; // LED6 blue + zero_to_epuck_buff[15] = 0; // LED8 red + zero_to_epuck_buff[16] = 0; // LED8 green + zero_to_epuck_buff[17] = 0; // LED8 blue } } +// The chip has two alternative addresses based on the AD1 pin. +void mpu9250_change_addr(void) { + if(imu_addr == MPU9250_ADDRESS_AD1_0) { + imu_addr = MPU9250_ADDRESS_AD1_1; + } else { + imu_addr = MPU9250_ADDRESS_AD1_0; + } + ioctl(fh, I2C_SLAVE, imu_addr); +} + +int read_reg(int file, uint8_t reg, int count, uint8_t *data) { + + if(write(file, ®, 1) != 1) { + mpu9250_change_addr(); + if(write(file, ®, 1) != 1) { + perror("imu write error"); + return -1; + } + } + + if(read(file, data, count) != count) { + mpu9250_change_addr(); + if(read(file, data, count) != count) { + printf("count=%d\n", count); + perror("imu read error"); + return -1; + } + } + + return 0; +} + +void calibrateAcc() { + int samplesCount=0, i=0; + // reset and send configuration first? + for(i=0; i 2) { + perror("update_robot_sensors_and_actuators: "); + return -1; + } else { + return 0; + } +} + void updateSensorsData() { + struct timeval timeout; fd_set readfds; int retval; - int trials = 0; - unsigned int bufIndex = 0; unsigned int bytesRead = 0; + uint8_t checksum = 0; + uint16_t i = 0; - memset(robotToPcBuff, 0x0, 255); - FD_ZERO(&readfds); - FD_SET(rfcommSock, &readfds); - write(rfcommSock, pcToRobotBuff, bytesToSend); - bytesRead = 0; - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(&lastTime3, NULL); - while(bytesRead < bytesToReceive) { - timeout.tv_sec=READ_TIMEOUT_SEC; // The timeout need to be set every time because the "select" may modify it. - timeout.tv_usec=READ_TIMEOUT_USEC; - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(&lastTime2, NULL); - retval = select(rfcommSock+1, &readfds, NULL, NULL, &timeout); - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(¤tTime2, NULL); - if(DEBUG_UPDATE_SENSORS_TIMING)std::cout << "[" << epuckname << "] " << "sensors data read in " << double((currentTime2.tv_sec*1000000 + currentTime2.tv_usec)-(lastTime2.tv_sec*1000000 + lastTime2.tv_usec))/1000000.0 << " sec" << std::endl; - if (retval>0) { - int n = read(rfcommSock, &robotToPcBuff[bytesRead], bytesToReceive-bytesRead); - //if(DEBUG_OTHERS)std::cout << "read " << n << " / " << bytesToReceive << " bytes" << std::endl; - bytesRead += n; - consecutiveReadTimeout = 0; - } else if(retval==0) { - if(DEBUG_COMMUNICATION_ERROR)std::cout << "[" << epuckname << "] " << "sensors read timeout" << std::endl; - consecutiveReadTimeout++; - break; - } else { - if(DEBUG_COMMUNICATION_ERROR)perror("sensors read error"); - break; - } + memset(epuck_to_zero_buff, 0x0, SENSORS_SIZE); + + checksum = 0; + for(i=0; i<(ACTUATORS_SIZE-1); i++) { + checksum ^= zero_to_epuck_buff[i]; } - if(bytesRead == bytesToReceive) { - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(¤tTime3, NULL); - if(DEBUG_UPDATE_SENSORS_TIMING)std::cout << "[" << epuckname << "] " << "sensors tot read time " << double((currentTime3.tv_sec*1000000 + currentTime3.tv_usec)-(lastTime3.tv_sec*1000000 + lastTime3.tv_usec))/1000000.0 << " sec" << std::endl; - bufIndex = 0; - if(enabledSensors[ACCELEROMETER]) { - accData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - accData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - accData[2] = (unsigned char)robotToPcBuff[bufIndex+4] | robotToPcBuff[bufIndex+5]<<8; - bufIndex += 6; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "acc: " << accData[0] << "," << accData[1] << "," << accData[2] << std::endl; - } - if(enabledSensors[MOTOR_SPEED]) { - motorSpeedData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - motorSpeedData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - bufIndex += 4; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "speed: " << motorSpeedData[0] << "," << motorSpeedData[1] << std::endl; - } - if(enabledSensors[FLOOR]) { - floorData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - floorData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - floorData[2] = (unsigned char)robotToPcBuff[bufIndex+4] | robotToPcBuff[bufIndex+5]<<8; - floorData[3] = (unsigned char)robotToPcBuff[bufIndex+6] | robotToPcBuff[bufIndex+7]<<8; - floorData[4] = (unsigned char)robotToPcBuff[bufIndex+8] | robotToPcBuff[bufIndex+9]<<8; - bufIndex += 10; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "floor: " << floorData[0] << "," << floorData[1] << "," << floorData[2] << "," << floorData[3] << "," << floorData[4] << std::endl; - } + zero_to_epuck_buff[ACTUATORS_SIZE-1] = checksum; + + update_robot_sensors_and_actuators(); + + // Verify the checksum (Longitudinal Redundancy Check) before interpreting the received sensors data. + checksum = 0; + for(i=0; i<(SENSORS_SIZE-1); i++) { + checksum ^= epuck_to_zero_buff[i]; + } + if(checksum == epuck_to_zero_buff[SENSORS_SIZE-1]) { if(enabledSensors[PROXIMITY]) { - proxData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - proxData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - proxData[2] = (unsigned char)robotToPcBuff[bufIndex+4] | robotToPcBuff[bufIndex+5]<<8; - proxData[3] = (unsigned char)robotToPcBuff[bufIndex+6] | robotToPcBuff[bufIndex+7]<<8; - proxData[4] = (unsigned char)robotToPcBuff[bufIndex+8] | robotToPcBuff[bufIndex+9]<<8; - proxData[5] = (unsigned char)robotToPcBuff[bufIndex+10] | robotToPcBuff[bufIndex+11]<<8; - proxData[6] = (unsigned char)robotToPcBuff[bufIndex+12] | robotToPcBuff[bufIndex+13]<<8; - proxData[7] = (unsigned char)robotToPcBuff[bufIndex+14] | robotToPcBuff[bufIndex+15]<<8; - bufIndex += 16; + proxData[0] = (unsigned char)epuck_to_zero_buff[0] | epuck_to_zero_buff[1]<<8; + proxData[1] = (unsigned char)epuck_to_zero_buff[2] | epuck_to_zero_buff[3]<<8; + proxData[2] = (unsigned char)epuck_to_zero_buff[4] | epuck_to_zero_buff[5]<<8; + proxData[3] = (unsigned char)epuck_to_zero_buff[6] | epuck_to_zero_buff[7]<<8; + proxData[4] = (unsigned char)epuck_to_zero_buff[8] | epuck_to_zero_buff[9]<<8; + proxData[5] = (unsigned char)epuck_to_zero_buff[10] | epuck_to_zero_buff[11]<<8; + proxData[6] = (unsigned char)epuck_to_zero_buff[12] | epuck_to_zero_buff[13]<<8; + proxData[7] = (unsigned char)epuck_to_zero_buff[14] | epuck_to_zero_buff[15]<<8; if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "prox: " << proxData[0] << "," << proxData[1] << "," << proxData[2] << "," << proxData[3] << "," << proxData[4] << "," << proxData[5] << "," << proxData[6] << "," << proxData[7] << std::endl; } - if(enabledSensors[MOTOR_POSITION]) { - motorPositionData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - motorPositionData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - bufIndex += 4; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "position: " << motorPositionData[0] << "," << motorPositionData[1] << std::endl; - } + // Prox ambient at index 16..31 if(enabledSensors[MICROPHONE]) { - micData[0] = (unsigned char)robotToPcBuff[bufIndex] | robotToPcBuff[bufIndex+1]<<8; - micData[1] = (unsigned char)robotToPcBuff[bufIndex+2] | robotToPcBuff[bufIndex+3]<<8; - micData[2] = (unsigned char)robotToPcBuff[bufIndex+4] | robotToPcBuff[bufIndex+5]<<8; - bufIndex += 6; + micData[0] = (unsigned char)epuck_to_zero_buff[32] | epuck_to_zero_buff[33]<<8; + micData[1] = (unsigned char)epuck_to_zero_buff[34] | epuck_to_zero_buff[35]<<8; + micData[2] = (unsigned char)epuck_to_zero_buff[36] | epuck_to_zero_buff[37]<<8; + micData[3] = (unsigned char)epuck_to_zero_buff[38] | epuck_to_zero_buff[39]<<8; if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "mic: " << micData[0] << "," << micData[1] << "," << micData[2] << std::endl; - } - } else { - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "discard the sensors data" << std::endl; - } - - if(enabledSensors[CAMERA]) { - char buff[2]; - int imageBytesToReceive = imageSize; - buff[0] = -'I'; - buff[1] = 0; - FD_ZERO(&readfds); - FD_SET(rfcommSock, &readfds); - write(rfcommSock, buff, 2); - bytesRead = 0; - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(&lastTime3, NULL); - while(bytesRead < imageBytesToReceive) { - timeout.tv_sec=READ_TIMEOUT_SEC; - timeout.tv_usec=READ_TIMEOUT_USEC; - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(&lastTime2, NULL); - retval = select(rfcommSock+1, &readfds, NULL, NULL, &timeout); - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(¤tTime2, NULL); - if(DEBUG_UPDATE_SENSORS_TIMING)std::cout << "[" << epuckname << "] " << "camera data read in " << double((currentTime2.tv_sec*1000000 + currentTime2.tv_usec)-(lastTime2.tv_sec*1000000 + lastTime2.tv_usec))/1000000.0 << " sec" << std::endl; - if (retval>0) { - int n = read(rfcommSock, &camData[bytesRead], imageBytesToReceive-bytesRead); - //if(DEBUG_OTHERS)std::cout << "[" << epuckname << "] " << "camera: read " << bytesRead << " / " << imageBytesToReceive << " bytes" << std::endl; - bytesRead += n; - consecutiveReadTimeout = 0; - } else if(retval==0) { - if(DEBUG_COMMUNICATION_ERROR)std::cout << "[" << epuckname << "] " << "camera read timeout" << std::endl; - consecutiveReadTimeout++; - break; - } else { - if(DEBUG_COMMUNICATION_ERROR)perror("camera read error"); - break; - } } - if(bytesRead == imageBytesToReceive) { - if(DEBUG_UPDATE_SENSORS_TIMING)gettimeofday(¤tTime3, NULL); - if(DEBUG_UPDATE_SENSORS_TIMING)std::cout << "camera tot read time " << double((currentTime3.tv_sec*1000000 + currentTime3.tv_usec)-(lastTime3.tv_sec*1000000 + lastTime3.tv_usec))/1000000.0 << " sec" << std::endl; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "camera read correctly" << std::endl; - newImageReceived = true; - } else { - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "wrong camera data" << std::endl; - newImageReceived = false; - } - + selectorData = epuck_to_zero_buff[40]&0x0F; + // Button at index 40 (4 most significant bits) + if(enabledSensors[MOTOR_POSITION]) { + motorPositionData[0] = (unsigned char)epuck_to_zero_buff[41] | epuck_to_zero_buff[42]<<8; + motorPositionData[1] = (unsigned char)epuck_to_zero_buff[43] | epuck_to_zero_buff[44]<<8; + if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "position: " << motorPositionData[0] << "," << motorPositionData[1] << std::endl; + } + tvRemoteData = epuck_to_zero_buff[45]; + } else { + if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "discard the sensors data" << std::endl; } - -// while(1) { -// char tempBuff[255]; -// timeout.tv_sec=0; -// timeout.tv_usec=300000; -// retval = select(rfcommSock+1, &readfds, NULL, NULL, &timeout); -// if (retval!=0) { -// int n = read(rfcommSock, &tempBuff[0], 255); -// if(DEBUG_OTHERS)std::cout << "[" << epuckname << "] " << "resync: read " << n << " bytes" << std::endl; -// } else { -// if(DEBUG_OTHERS)std::cout << "[" << epuckname << "] " << "resync: read timeout" << std::endl; -// break; -// } -// } - -} -void RGB565toRGB888(int width, int height, unsigned char *src, unsigned char *dst) { - int line, column; - int index_src=0, index_dst=0; + // if(enabledSensors[FLOOR]) { + // floorData[0] = ... + // } + + if(enabledSensors[IMU]) { + ioctl(fh, I2C_SLAVE, imu_addr); + + read_reg(fh, ACCEL_XOUT_H, 6, accData); + read_reg(fh, GYRO_XOUT_H, 6, gyroData); - for (line = 0; line < height; ++line) { - for (column = 0; column < width; ++column) { - dst[index_dst++] = (unsigned char)(src[index_src] & 0xF8); - dst[index_dst++] = (unsigned char)((src[index_src]&0x07)<<5) | (unsigned char)((src[index_src+1]&0xE0)>>3); - dst[index_dst++] = (unsigned char)((src[index_src+1]&0x1F)<<3); - index_src+=2; - } - } + accValue[0] = (accData[1] + (accData[0]<<8));// MPU9250 big-endian + accValue[1] = (accData[3] + (accData[2]<<8)); + accValue[2] = (accData[5] + (accData[4]<<8)); + + gyroValue[0] = (gyroData[1] + (gyroData[0]<<8)); + gyroValue[1] = (gyroData[3] + (gyroData[2]<<8)); + gyroValue[2] = (gyroData[5] + (gyroData[4]<<8)); + + if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "acc: " << accValue[0] << "," << accValue[1] << "," << accValue[2] << std::endl; + if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "gyro: " << gyroValue[0] << "," << gyroValue[1] << "," << gyroValue[2] << std::endl; + } + + if(debug_enabled) { + debug_count++; + if(debug_count == 50) { + debug_count = 0; + std::cout << "[" << epuckname << "] " << "prox: " << proxData[0] << "," << proxData[1] << "," << proxData[2] << "," << proxData[3] << "," << proxData[4] << "," << proxData[5] << "," << proxData[6] << "," << proxData[7] << std::endl; + std::cout << "[" << epuckname << "] " << "mic: " << micData[0] << "," << micData[1] << "," << micData[2] << std::endl; + std::cout << "[" << epuckname << "] " << "position: " << motorPositionData[0] << "," << motorPositionData[1] << std::endl; + } + } + } void updateRosInfo() { @@ -929,93 +830,94 @@ void updateRosInfo() { br.sendTransform(odomTrans); } - if(enabledSensors[ACCELEROMETER]) { + if(enabledSensors[IMU]) { std::stringstream ss; ss << epuckname << "/base_link"; - accelMsg.header.frame_id = ss.str(); - accelMsg.header.stamp = ros::Time::now(); - accelMsg.linear_acceleration.x = (accData[1]-2048.0)/800.0*9.81; // 1 g = about 800, then transforms in m/s^2. - accelMsg.linear_acceleration.y = (accData[0]-2048.0)/800.0*9.81; - accelMsg.linear_acceleration.z = (accData[2]-2048.0)/800.0*9.81; - accelMsg.linear_acceleration_covariance[0] = 0.01; - accelMsg.linear_acceleration_covariance[1] = 0.0; - accelMsg.linear_acceleration_covariance[2] = 0.0; - accelMsg.linear_acceleration_covariance[3] = 0.0; - accelMsg.linear_acceleration_covariance[4] = 0.01; - accelMsg.linear_acceleration_covariance[5] = 0.0; - accelMsg.linear_acceleration_covariance[6] = 0.0; - accelMsg.linear_acceleration_covariance[7] = 0.0; - accelMsg.linear_acceleration_covariance[8] = 0.01; - if(DEBUG_ACCELEROMETER)std::cout << "[" << epuckname << "] " << "accel raw: " << accData[0] << ", " << accData[1] << ", " << accData[2] << std::endl; - if(DEBUG_ACCELEROMETER)std::cout << "[" << epuckname << "] " << "accel (m/s2): " << ((accData[0]-2048.0)/800.0*9.81) << ", " << ((accData[1]-2048.0)/800.0*9.81) << ", " << ((accData[2]-2048.0)/800.0*9.81) << std::endl; - accelMsg.angular_velocity.x = 0; - accelMsg.angular_velocity.y = 0; - accelMsg.angular_velocity.z = 0; - accelMsg.angular_velocity_covariance[0] = 0.01; - accelMsg.angular_velocity_covariance[1] = 0.0; - accelMsg.angular_velocity_covariance[2] = 0.0; - accelMsg.angular_velocity_covariance[3] = 0.0; - accelMsg.angular_velocity_covariance[4] = 0.01; - accelMsg.angular_velocity_covariance[5] = 0.0; - accelMsg.angular_velocity_covariance[6] = 0.0; - accelMsg.angular_velocity_covariance[7] = 0.0; - accelMsg.angular_velocity_covariance[8] = 0.01; + imuMsg.header.frame_id = ss.str(); + imuMsg.header.stamp = ros::Time::now(); + imuMsg.linear_acceleration.x = (accValue[0]-accOffset[0]) * STANDARD_GRAVITY * ACC_RAW2G; // m/s^2 + imuMsg.linear_acceleration.y = (accValue[1]-accOffset[1]) * STANDARD_GRAVITY * ACC_RAW2G; + imuMsg.linear_acceleration.z = (accValue[2]-accOffset[2]+GRAVITY_MPU9250) * STANDARD_GRAVITY * ACC_RAW2G; + imuMsg.linear_acceleration_covariance[0] = 0.01; + imuMsg.linear_acceleration_covariance[1] = 0.0; + imuMsg.linear_acceleration_covariance[2] = 0.0; + imuMsg.linear_acceleration_covariance[3] = 0.0; + imuMsg.linear_acceleration_covariance[4] = 0.01; + imuMsg.linear_acceleration_covariance[5] = 0.0; + imuMsg.linear_acceleration_covariance[6] = 0.0; + imuMsg.linear_acceleration_covariance[7] = 0.0; + imuMsg.linear_acceleration_covariance[8] = 0.01; + if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "accel raw: " << accValue[0] << ", " << accValue[1] << ", " << accValue[2] << std::endl; + if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "accel (m/s2): " << imuMsg.linear_acceleration.x << ", " << imuMsg.linear_acceleration.y << ", " << imuMsg.linear_acceleration.z << std::endl; + imuMsg.angular_velocity.x = (gyroValue[0] - gyroOffset[0]) * DEG2RAD(GYRO_RAW2DPS); // rad/s + imuMsg.angular_velocity.y = (gyroValue[1] - gyroOffset[1]) * DEG2RAD(GYRO_RAW2DPS); + imuMsg.angular_velocity.z = (gyroValue[2] - gyroOffset[2]) * DEG2RAD(GYRO_RAW2DPS); + imuMsg.angular_velocity_covariance[0] = 0.01; + imuMsg.angular_velocity_covariance[1] = 0.0; + imuMsg.angular_velocity_covariance[2] = 0.0; + imuMsg.angular_velocity_covariance[3] = 0.0; + imuMsg.angular_velocity_covariance[4] = 0.01; + imuMsg.angular_velocity_covariance[5] = 0.0; + imuMsg.angular_velocity_covariance[6] = 0.0; + imuMsg.angular_velocity_covariance[7] = 0.0; + imuMsg.angular_velocity_covariance[8] = 0.01; geometry_msgs::Quaternion odomQuat = tf::createQuaternionMsgFromYaw(0); - accelMsg.orientation = odomQuat; - accelMsg.orientation_covariance[0] = 0.01; - accelMsg.orientation_covariance[1] = 0.0; - accelMsg.orientation_covariance[2] = 0.0; - accelMsg.orientation_covariance[3] = 0.0; - accelMsg.orientation_covariance[4] = 0.01; - accelMsg.orientation_covariance[5] = 0.0; - accelMsg.orientation_covariance[6] = 0.0; - accelMsg.orientation_covariance[7] = 0.0; - accelMsg.orientation_covariance[8] = 0.01; - accelPublisher.publish(accelMsg); - } - if(enabledSensors[MOTOR_SPEED]) { - std::stringstream ss; - ss << epuckname << "/base_link"; - motorSpeedMsg.header.frame_id = ss.str(); - motorSpeedMsg.header.stamp = ros::Time::now(); - motorSpeedMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - motorSpeedMsg.pose.position.x = 0.15; - motorSpeedMsg.pose.position.y = 0; - motorSpeedMsg.pose.position.z = 0.15; - geometry_msgs::Quaternion odomQuat = tf::createQuaternionMsgFromYaw(0); - motorSpeedMsg.pose.orientation = odomQuat; - motorSpeedMsg.scale.z = 0.01; - motorSpeedMsg.color.a = 1.0; - motorSpeedMsg.color.r = 1.0; - motorSpeedMsg.color.g = 1.0; - motorSpeedMsg.color.b = 1.0; - ss.str(""); - ss << "speed: [" << motorSpeedData[0] << ", " << motorSpeedData[1] << "]"; - motorSpeedMsg.text = ss.str(); - motorSpeedPublisher.publish(motorSpeedMsg); + imuMsg.orientation = odomQuat; + imuMsg.orientation_covariance[0] = 0.01; + imuMsg.orientation_covariance[1] = 0.0; + imuMsg.orientation_covariance[2] = 0.0; + imuMsg.orientation_covariance[3] = 0.0; + imuMsg.orientation_covariance[4] = 0.01; + imuMsg.orientation_covariance[5] = 0.0; + imuMsg.orientation_covariance[6] = 0.0; + imuMsg.orientation_covariance[7] = 0.0; + imuMsg.orientation_covariance[8] = 0.01; + imuPublisher.publish(imuMsg); } + + // if(enabledSensors[MOTOR_SPEED]) { + // std::stringstream ss; + // ss << epuckname << "/base_link"; + // motorSpeedMsg.header.frame_id = ss.str(); + // motorSpeedMsg.header.stamp = ros::Time::now(); + // motorSpeedMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + // motorSpeedMsg.pose.position.x = 0.15; + // motorSpeedMsg.pose.position.y = 0; + // motorSpeedMsg.pose.position.z = 0.15; + // geometry_msgs::Quaternion odomQuat = tf::createQuaternionMsgFromYaw(0); + // motorSpeedMsg.pose.orientation = odomQuat; + // motorSpeedMsg.scale.z = 0.01; + // motorSpeedMsg.color.a = 1.0; + // motorSpeedMsg.color.r = 1.0; + // motorSpeedMsg.color.g = 1.0; + // motorSpeedMsg.color.b = 1.0; + // ss.str(""); + // ss << "speed: [" << motorSpeedData[0] << ", " << motorSpeedData[1] << "]"; + // motorSpeedMsg.text = ss.str(); + // motorSpeedPublisher.publish(motorSpeedMsg); + // } - if(enabledSensors[FLOOR]) { - std::stringstream ss; - ss << epuckname << "/base_link"; - floorMsg.header.frame_id = ss.str(); - floorMsg.header.stamp = ros::Time::now(); - floorMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - floorMsg.pose.position.x = 0.15; - floorMsg.pose.position.y = 0; - floorMsg.pose.position.z = 0.13; - geometry_msgs::Quaternion odomQuat = tf::createQuaternionMsgFromYaw(0); - floorMsg.pose.orientation = odomQuat; - floorMsg.scale.z = 0.01; - floorMsg.color.a = 1.0; - floorMsg.color.r = 1.0; - floorMsg.color.g = 1.0; - floorMsg.color.b = 1.0; - ss.str(""); - ss << "floor: [" << floorData[0] << ", " << floorData[1] << ", " << floorData[2] << ", " << floorData[3] << ", " << floorData[4] << "]"; - floorMsg.text = ss.str(); - floorPublisher.publish(floorMsg); - } + // if(enabledSensors[FLOOR]) { + // std::stringstream ss; + // ss << epuckname << "/base_link"; + // floorMsg.header.frame_id = ss.str(); + // floorMsg.header.stamp = ros::Time::now(); + // floorMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + // floorMsg.pose.position.x = 0.15; + // floorMsg.pose.position.y = 0; + // floorMsg.pose.position.z = 0.13; + // geometry_msgs::Quaternion odomQuat = tf::createQuaternionMsgFromYaw(0); + // floorMsg.pose.orientation = odomQuat; + // floorMsg.scale.z = 0.01; + // floorMsg.color.a = 1.0; + // floorMsg.color.r = 1.0; + // floorMsg.color.g = 1.0; + // floorMsg.color.b = 1.0; + // ss.str(""); + // ss << "floor: [" << floorData[0] << ", " << floorData[1] << ", " << floorData[2] << ", " << floorData[3] << ", " << floorData[4] << "]"; + // floorMsg.text = ss.str(); + // floorPublisher.publish(floorMsg); + // } if(enabledSensors[MICROPHONE]) { std::stringstream ss; @@ -1034,30 +936,11 @@ void updateRosInfo() { microphoneMsg.color.g = 1.0; microphoneMsg.color.b = 1.0; ss.str(""); - ss << "mic: [" << micData[0] << ", " << micData[1] << ", " << micData[2] << "]"; + ss << "mic: [" << micData[0] << ", " << micData[1] << ", " << micData[2] << ", " << micData[3] << "]"; microphoneMsg.text = ss.str(); microphonePublisher.publish(microphoneMsg); } - - if(enabledSensors[CAMERA]) { - if(newImageReceived) { - newImageReceived = false; - cv::Mat rgb888; - cv_bridge::CvImage out_msg; - out_msg.header.stamp = ros::Time::now();; // Same timestamp and tf frame as input image - if(camMode == CAM_MODE_RGB) { - rgb888 = cv::Mat(camHeight, camWidth, CV_8UC3); - RGB565toRGB888(camWidth, camHeight, &camData[3], rgb888.data); - out_msg.encoding = sensor_msgs::image_encodings::RGB8; - } else { - rgb888 = cv::Mat(camHeight, camWidth, CV_8UC1); - rgb888.data = &camData[3]; - out_msg.encoding = sensor_msgs::image_encodings::MONO8; - } - out_msg.image = rgb888; - imagePublisher.publish(out_msg.toImageMsg()); - } - } + } void handlerVelocity(const geometry_msgs::Twist::ConstPtr& msg) { @@ -1080,25 +963,24 @@ void handlerVelocity(const geometry_msgs::Twist::ConstPtr& msg) { void handlerLED(const std_msgs::UInt8MultiArray::ConstPtr& msg) { // Controls the state of each LED on the standard robot - for (int i = 0; i < LED_NUMBER; i++) + for (int i = 0; i < LEDS_NUM; i++) ledState[i] = msg->data[i]; changedActuators[LEDS] = true; if(DEBUG_LED_RECEIVED) { std::cout << "[" << epuckname << "] " << "new LED status: " << std::endl; - for (int i = 0; i < LED_NUMBER; i++) + for (int i = 0; i < LEDS_NUM; i++) std::cout << ledState[i] << ", "; } } int main(int argc,char *argv[]) { - - int robotId = 0; + double init_xpos, init_ypos, init_theta; int rosRate = 0; - unsigned int bufIndex = 0; int i = 0; - std::string epuckAddress(""); + + zero_to_epuck_buff[4] = 0; // Speaker => 0 = no sound. /** * The ros::init() function needs to see argc and argv so that it can perform @@ -1120,114 +1002,50 @@ int main(int argc,char *argv[]) { ros::NodeHandle np("~"); // Private. ros::NodeHandle n; // Public. - np.getParam("epuck_id", robotId); - np.param("epuck_address", epuckAddress, ""); np.param("epuck_name", epuckname, "epuck"); np.param("xpos", init_xpos, 0.0); np.param("ypos", init_ypos, 0.0); np.param("theta", init_theta, 0.0); - np.param("accelerometer", enabledSensors[ACCELEROMETER], false); + np.param("imu", enabledSensors[IMU], false); np.param("motor_speed", enabledSensors[MOTOR_SPEED], false); np.param("floor", enabledSensors[FLOOR], false); np.param("proximity", enabledSensors[PROXIMITY], false); np.param("motor_position", enabledSensors[MOTOR_POSITION], false); np.param("microphone", enabledSensors[MICROPHONE], false); - np.param("camera", enabledSensors[CAMERA], false); - np.param("cam_width", camWidth, 160); - np.param("cam_height", camHeight, 2); - np.param("cam_zoom", camZoom, 1); - np.param("cam_mode", camMode, 0); - np.param("cam_x_offset", camXoffset, 240); - np.param("cam_y_offset", camYoffset, 239); - //np.param("ros_rate", rosRate, 7); - - if(camWidth < 0 || camWidth > 640) { - camWidth = 160; - } - if(camHeight < 0 || camHeight > 480) { - camHeight = 2; - } - if(camZoom < 0 || camZoom > 8) { - camZoom = 1; - } - if(camMode < 0 || camMode > 1) { - camMode = CAM_MODE_GRAY; - } - if(camXoffset < 0 || camXoffset > 640) { - camXoffset = (640-camWidth*camZoom)/2; // Center the slice. - } - if(camYoffset < 0 || camYoffset > 480) { - camYoffset = (480-camHeight*camZoom)/2; // Center the slice. - } - - if(camWidth*camHeight*(camMode+1) > CAM_MAX_BUFFER_SIZE) { // If the parameters exceed the maximum buffer then use default values for all params. - std::cerr << "[" << epuckname << "] " << "Camera parameters exceed max buffer size, using default values" << std::endl; - camWidth = 160; - camHeight = 2; - camZoom = 1; - camMode = CAM_MODE_GRAY; - camXoffset = (640-camWidth*camZoom)/2; - camYoffset = (480-camWidth*camZoom)/2; - } - + np.param("ros_rate", rosRate, 20); + np.param("debug", debug_enabled, false); + if(DEBUG_ROS_PARAMS) { - std::cout << "[" << epuckname << "] " << "epuck id: " << robotId << std::endl; - std::cout << "[" << epuckname << "] " << "epuck address: " << epuckAddress << std::endl; std::cout << "[" << epuckname << "] " << "epuck name: " << epuckname << std::endl; std::cout << "[" << epuckname << "] " << "init pose: " << init_xpos << ", " << init_ypos << ", " << theta << std::endl; - std::cout << "[" << epuckname << "] " << "accelerometer enabled: " << enabledSensors[ACCELEROMETER] << std::endl; + std::cout << "[" << epuckname << "] " << "imu enabled: " << enabledSensors[IMU] << std::endl; std::cout << "[" << epuckname << "] " << "motor speed enabled: " << enabledSensors[MOTOR_SPEED] << std::endl; std::cout << "[" << epuckname << "] " << "floor enabled: " << enabledSensors[FLOOR] << std::endl; std::cout << "[" << epuckname << "] " << "proximity enabled: " << enabledSensors[PROXIMITY] << std::endl; std::cout << "[" << epuckname << "] " << "motor position enabled: " << enabledSensors[MOTOR_POSITION] << std::endl; std::cout << "[" << epuckname << "] " << "microphone enabled: " << enabledSensors[MICROPHONE] << std::endl; - std::cout << "[" << epuckname << "] " << "camera enabled: " << enabledSensors[CAMERA] << std::endl; - //std::cout << "[" << epuckname << "] " << "ros rate: " << rosRate << std::endl; - std::cout << "[" << epuckname << "] " << "image size: " << camWidth << " x " << camHeight << std::endl; - std::cout << "[" << epuckname << "] " << "image zoom: " << camZoom << std::endl; - std::cout << "[" << epuckname << "] " << "image mode: " << (camMode?"RGB":"GRAY") << std::endl; - std::cout << "[" << epuckname << "] " << "image offset: " << camXoffset << ", " << camYoffset << std::endl; + std::cout << "[" << epuckname << "] " << "ros rate: " << rosRate << std::endl; + std::cout << "[" << epuckname << "] " << "debug enabled: " << debug_enabled << std::endl; } - if(epuckAddress.compare("")==0) { // Search the robot id - if(initConnectionWithRobotId(robotId)<0) { - return -1; - } - } else { // Connect directly to the address - if(initConnectionWithRobotAddress(epuckAddress.c_str())<0) { - return -1; - } + + if(initConnectionWithRobot()<0) { + return -1; } - bufIndex = 0; - bytesToReceive = 0; - if(enabledSensors[ACCELEROMETER]) { - //if(DEBUG)std::cout << "acc enabled" << std::endl; - pcToRobotBuff[bufIndex] = -'a'; - bufIndex++; - bytesToReceive += 6; - - accelPublisher = n.advertise("accel", 10); + if(enabledSensors[IMU]) { + ioctl(fh, I2C_SLAVE, imu_addr); + calibrateAcc(); + calibrateGyro(); + imuPublisher = n.advertise("imu", 10); } if(enabledSensors[MOTOR_SPEED]) { - pcToRobotBuff[bufIndex] = -'E'; - bufIndex++; - bytesToReceive += 4; - motorSpeedPublisher = n.advertise("motor_speed", 10); } if(enabledSensors[FLOOR]) { - pcToRobotBuff[bufIndex] = -'M'; - bufIndex++; - bytesToReceive += 10; - floorPublisher = n.advertise("floor", 10); } if(enabledSensors[PROXIMITY]) { - pcToRobotBuff[bufIndex] = -'N'; - bufIndex++; - bytesToReceive += 16; - for(i=0; i<8; i++) { /** * The advertise() function is how you tell ROS that you want to @@ -1263,27 +1081,13 @@ int main(int argc,char *argv[]) { laserPublisher = n.advertise("scan", 10); } if(enabledSensors[MOTOR_POSITION]) { - pcToRobotBuff[bufIndex] = -'Q'; - bufIndex++; - bytesToReceive += 4; - odomPublisher = n.advertise("odom", 10); currentTime = ros::Time::now(); lastTime = ros::Time::now(); } if(enabledSensors[MICROPHONE]) { - pcToRobotBuff[bufIndex] = -'u'; - bufIndex++; - bytesToReceive += 6; - microphonePublisher = n.advertise("microphone", 10); } - if(bufIndex == 0) { - std::cerr << "[" << epuckname << "] " << "No sensors enabled!" << std::endl; - return -1; - } - pcToRobotBuff[bufIndex] = 0; // Terminate the command sequence; the camera image will be handled separately. - bytesToSend = bufIndex + 1; /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -1302,57 +1106,19 @@ int main(int argc,char *argv[]) { */ cmdVelSubscriber = n.subscribe("mobile_base/cmd_vel", 10, handlerVelocity); cmdLedSubscriber = n.subscribe("mobile_base/cmd_led", 10, handlerLED); - - if(enabledSensors[CAMERA]) { - imageSize = camWidth*camHeight*(camMode+1)+3; // The image data header contains "mode", "width", "height" in the first 3 bytes. - camData = (unsigned char *) malloc (imageSize); - - // Configure camera params. - char buff[30]; - struct timeval timeout; - fd_set readfds; - int retval; - int bytesRead; - memset(buff, 0x0, 30); - sprintf(buff,"J,%d,%d,%d,%d,%d,%d\r", camMode, camWidth, camHeight, camZoom, camXoffset, camYoffset); - FD_ZERO(&readfds); - FD_SET(rfcommSock, &readfds); - write(rfcommSock, buff, strlen(buff)); - bytesRead = 0; - while(bytesRead < 3) { - timeout.tv_sec=READ_TIMEOUT_SEC; - timeout.tv_usec=READ_TIMEOUT_USEC; - retval = select(rfcommSock+1, &readfds, NULL, NULL, &timeout); - if (retval!=0) { - int n = read(rfcommSock, &buff[bytesRead], 3-bytesRead); - if(DEBUG_CAMERA_INIT)std::cout << "[" << epuckname << "] " << "cam init: read " << bytesRead << " / " << 3 << "bytes" << std::endl; - bytesRead += n; - } else { - if(DEBUG_CAMERA_INIT)std::cout << "[" << epuckname << "] " << "cam init: read timeout" << std::endl; - break; - } - } - if(bytesRead == 3) { - if(DEBUG_CAMERA_INIT)std::cout << "[" << epuckname << "] " << "camera init correctly (" << buff[0] << buff[1] << buff[2] << ")" << std::endl; - imagePublisher = n.advertise("camera", 1); - } else { - if(DEBUG_CAMERA_INIT)std::cout << "[" << epuckname << "] " << "cannot init camera" << std::endl; - enabledSensors[CAMERA] = false; - } - } - + theta = init_theta; xPos = init_xpos; yPos = init_ypos; - //ros::Rate loop_rate(rosRate); + ros::Rate loop_rate(rosRate); while (ros::ok()) { updateSensorsData(); updateRosInfo(); updateActuators(); ros::spinOnce(); - //loop_rate.sleep(); // Do not call "sleep" otherwise the bluetooth communication will hang. + loop_rate.sleep(); // Do not call "sleep" otherwise the bluetooth communication will hang. // We communicate as fast as possible, this shouldn't be a problem... if(consecutiveReadTimeout >= MAX_CONSECUTIVE_TIMEOUT) { // We have connection problems, stop here. break; @@ -1361,10 +1127,6 @@ int main(int argc,char *argv[]) { closeConnection(); - if(enabledSensors[CAMERA]) { - free(camData); - } - }