Merge pull request #1 from ptroja/master

Support for sonar interface and some minor cleanups

Makefile

@@ -1,6 +1,6 @@
 .PHONY: all clean
 
-PARAMS=-Pplayer3_ada -XPlayer3_Ada_Include_Test=No -XPlayer3_Ada_Link=Static_Library -XPlayer3_Ada_Prefix=${HOME}/local
+PARAMS=-Pplayer3_ada -XPlayer3_Ada_Include_Test=No -XPlayer3_Ada_Link=Static_Library
 
 all:
 	gprbuild ${PARAMS}

player-aux.ads

@@ -1,4 +1,4 @@
- --  Auxiliary things for interfacing with C
+--  Auxiliary things for interfacing with C
 
 with Interfaces.C;
 with Interfaces.C.Strings;

player-c.c

@@ -18,6 +18,19 @@ playerc_error_mode ()
   return PLAYERC_ERROR_MODE;
 }
 
+int
+playerc_device_get_fresh(playerc_device_t *device)
+{
+  return device->fresh;
+}
+
+void
+playerc_device_set_fresh(playerc_device_t *device,
+                         int fresh)
+{
+  device->fresh = fresh;
+}
+
 int
 player_ada_graphics2d_set_color(playerc_graphics2d_t *proxy,
                                  int a, int r, int g, int b)
@@ -103,6 +116,18 @@ player_ada_laser_get_scan_id(playerc_laser_t *proxy)
     return proxy->scan_id;
 }
 
+double
+player_ada_laser_get_min_right(playerc_laser_t *proxy)
+{
+    return proxy->min_right;
+}
+
+double
+player_ada_laser_get_min_left(playerc_laser_t *proxy)
+{
+    return proxy->min_left;
+}
+
 void
 player_ada_laser_get_robot_pose(playerc_laser_t *proxy,
 				double *x, double *y, double *a)
@@ -112,6 +137,40 @@ player_ada_laser_get_robot_pose(playerc_laser_t *proxy,
     *a = proxy->robot_pose[2];
 }
 
+int
+player_ada_sonar_get_pose_count(playerc_sonar_t *proxy)
+{
+    return proxy->pose_count;
+}
+
+void
+player_ada_sonar_get_pose(playerc_sonar_t *proxy,
+				int i,
+				double *px, double *py, double *pz,
+				double *proll, double *ppitch, double *pyaw)
+{
+    *px = proxy->poses[i].px;
+    *py = proxy->poses[i].py;
+    *pz = proxy->poses[i].pz;
+    *proll = proxy->poses[i].proll;
+    *ppitch = proxy->poses[i].ppitch;
+    *pyaw = proxy->poses[i].pyaw;
+}
+
+int
+player_ada_sonar_get_scan_count(playerc_sonar_t *proxy,
+				int *ranges_count)
+{
+    return proxy->scan_count;
+}
+
+double
+player_ada_sonar_get_scan(playerc_sonar_t *proxy,
+				int i)
+{
+    return proxy->scan[i];
+}
+
 void
 player_ada_position2d_get_geom(playerc_position2d_t *proxy,
 			       double *x, double *y, double *a)

player-client.adb

@@ -1,10 +1,7 @@
-
-
 with Player.Aux;
 
 with Interfaces.C;
 with Interfaces.C.Strings; use Interfaces;
-with Ada.Text_IO; use Ada.Text_IO;
 
 package body Player.Client is
 

player-gps.ads

@@ -1,4 +1,4 @@
- --  GPS proxy interface
+--  GPS proxy interface
 
 with Player.Client;
 with Player.Interfaces;

player-interfaces.adb

@@ -1,4 +1,4 @@
- 
+with Interfaces.C;
 
 package body Player.Interfaces is
 
@@ -77,4 +77,26 @@ package body Player.Interfaces is
       end if;
    end Finalize;
 
+   ---------------
+   -- Get_Fresh --
+   ---------------
+
+   function Get_Fresh (This : in Object) return C.Int is
+      function Internal (This : Types.Handle) return C.Int;
+      pragma Import (C, Internal, "playerc_device_get_fresh");
+   begin
+      return Internal(-This);
+   end Get_Fresh;
+
+   ---------------
+   -- Set_Fresh --
+   ---------------
+
+   procedure Set_Fresh (This : in out Object; Fresh : in C.Int) is
+      procedure Internal (This : Types.Handle; Fresh : in C.Int);
+      pragma Import (C, Internal, "playerc_device_set_fresh");
+   begin
+      Internal(-This, Fresh);
+   end Set_Fresh;
+
 end Player.Interfaces;

player-interfaces.ads

@@ -1,4 +1,4 @@
- 
+
 
 --  Root class for interfaces.
 --  When extending these, each interface should provide its own Subscribe and
@@ -8,6 +8,8 @@ with Player.Client;
 with Player.Types;
 
 with Ada.Finalization;
+with Interfaces;
+use Interfaces;
 
 package Player.Interfaces is
 
@@ -43,6 +45,10 @@ package Player.Interfaces is
 
    procedure Unsubscribe  (This : in out Object);
 
+   function  Get_Fresh    (This : in Object) return C.Int;
+
+   procedure Set_Fresh    (This : in out Object; Fresh : in C.Int);
+
 private
 
    type Object is abstract new Ada.Finalization.Limited_Controlled with record

player-laser.adb

@@ -237,6 +237,28 @@ package body Player.Laser is
       return Integer (Internal (-This));
    end Get_Scan_Id;
 
+   -------------------
+   -- Get_Min_Right --
+   -------------------
+
+   function Get_Min_Right (This : in Object) return Double is
+      function Internal (This : Types.Handle) return C.Double;
+      pragma Import (C, Internal, "player_ada_laser_get_min_right");
+   begin
+      return Double (Internal (-This));
+   end Get_Min_Right;
+
+   ------------------
+   -- Get_Min_Left --
+   ------------------
+
+   function Get_Min_Left (This : in Object) return Double is
+      function Internal (This : Types.Handle) return C.Double;
+      pragma Import (C, Internal, "player_ada_laser_get_min_left");
+   begin
+      return Double (Internal (-This));
+   end Get_Min_Left;
+
    --------------------
    -- Get_Robot_Pose --
    --------------------

player-laser.ads

@@ -1,4 +1,4 @@
- --  Laser proxy interface
+--  Laser proxy interface
 
 with Player.Client;
 with Player.Interfaces;
@@ -93,6 +93,12 @@ package Player.Laser is
    function Get_Scan_Id (This : in Object) return Integer;
    --  Serial number that can be used to detect a new laser reading.
 
+   function Get_Min_Right (This : in Object) return Double;
+   --  Minimum range, in meters, in the right half of the scan.
+
+   function Get_Min_Left (This : in Object) return Double;
+   --  Minimum range, in meters, in the left half of the scan.
+
    function Get_Robot_Pose (This : in Object) return Pose;
    --  Pose when the reading was taken
 

player-sonar.adb

@@ -0,0 +1,126 @@
+with player.Aux;
+
+with Interfaces; use Interfaces;
+
+package body Player.Sonar is
+
+   ------------
+   -- Create --
+   ------------
+
+   procedure Create
+     (This  : in out Object;
+      Conn  : in     Client.Connection_Type;
+      Index : in     Natural := 0)
+   is
+      function C_Create (Conn : in Types.Handle; Index : in C.int)
+                         return Types.Handle;
+      pragma Import (C, C_Create, "playerc_sonar_create");
+   begin
+      Interfaces.Create (Interfaces.Object (This), Conn, Index);
+      Set_Handle (This, C_Create (Client.Get_Handle (Conn), C.int (Index)));
+   end Create;
+
+   ---------------
+   -- Subscribe --
+   ---------------
+
+   procedure Subscribe (This : in out Object; Mode : in Access_Modes) is
+      function C_Subscribe (This : in Types.Handle; Mode : in Access_Modes)
+                            return C.int;
+      pragma Import (C, C_Subscribe, "playerc_sonar_subscribe");
+   begin
+      Aux.Check (C_Subscribe (-This, Mode));
+      Interfaces.Subscribe (Interfaces.Object (This), Mode);
+   end Subscribe;
+
+   -----------------
+   -- Unsubscribe --
+   -----------------
+
+   procedure Unsubscribe (This : in out Object) is
+      function C_Unsubscribe (This : in Types.Handle) return C.Int;
+      pragma Import (C, C_Unsubscribe, "playerc_sonar_unsubscribe");
+   begin
+      Aux.Check (C_Unsubscribe (-This));
+      Interfaces.Unsubscribe (Interfaces.Object (This));
+   end Unsubscribe;
+
+   -----------------
+   -- Update_Geom --
+   -----------------
+
+   procedure Update_Geom (This : in out Object) is
+      function Internal (This : Types.Handle) return C.Int;
+      pragma Import (C, Internal, "playerc_sonar_get_geom");
+   begin
+      Aux.Check (Internal (-This));
+   end Update_Geom;
+
+   --------------------
+   -- Get_Pose_Count --
+   --------------------
+
+   function Get_Pose_Count (This : in Object) return Natural is
+      function Internal (This : Types.Handle) return C.Int;
+      pragma Import (C, Internal, "player_ada_sonar_get_pose_count");
+   begin
+      return Natural( Internal (-This));
+   end Get_Pose_Count;
+
+   --------------
+   -- Get_Pose --
+   --------------
+
+   function Get_Pose (This : in Object; Idx : in Positive) return Pose_3d is
+      procedure Internal (This : Types.Handle;
+                          Index : in C.Int;
+                          X, Y, Z, Roll, Pitch, Yaw : out C.Double);
+      pragma Import (C, Internal, "player_ada_sonar_get_pose");
+      P : Pose_3d;
+   begin
+      Internal (-This,
+                C.Int (Idx - 1),
+                C.Double (P.px),
+                C.Double (P.py),
+                C.Double (P.pz),
+                C.Double (P.proll),
+                C.Double (P.ppitch),
+                C.Double (P.pyaw));
+      return P;
+   end Get_Pose;
+
+   --------------------
+   -- Get_Scan_Count --
+   --------------------
+
+   function Get_Scan_Count (This : in Object) return Natural is
+      function Internal (This : Types.Handle) return C.Int;
+      pragma Import (C, Internal, "player_ada_sonar_get_scan_count");
+   begin
+      return Natural (Internal (-This));
+   end Get_Scan_Count;
+
+   --------------
+   -- Get_Scan --
+   --------------
+
+   function Get_Scan (This : in Object; Idx : in Positive) return Double is
+      function Internal (This  : Types.Handle;
+                         Index : C.Int) return C.Double;
+      pragma Import (C, Internal, "player_ada_sonar_get_scan");
+   begin
+      return Double (Internal (-This, C.Int(Idx - 1)));
+   end Get_Scan;
+
+   -------------
+   -- Destroy --
+   -------------
+
+   procedure Destroy (This : in out Object) is
+   begin
+      Destroy_Handle (-This);
+      Clear_Handle (This);
+   end Destroy;
+
+end Player.Sonar;