diff --git a/external/controller/external_controller.gpr b/external/controller/external_controller.gpr index d8c06f5..0e01725 100644 --- a/external/controller/external_controller.gpr +++ b/external/controller/external_controller.gpr @@ -4,8 +4,10 @@ project External_Controller is "src/tcp_client/**", "src/backend_thread/**", "src/webots_thread/**", + "src/path/**", "src/knowledge/**", "src/vendor/**"); + for Object_Dir use "obj"; for Main use ("main.adb"); for Exec_Dir use "."; diff --git a/external/controller/main b/external/controller/main new file mode 100755 index 0000000..07132ec Binary files /dev/null and b/external/controller/main differ diff --git a/external/controller/src/.#main.adb# b/external/controller/src/.#main.adb# new file mode 100644 index 0000000..c76e790 --- /dev/null +++ b/external/controller/src/.#main.adb# @@ -0,0 +1,121 @@ +with Ada.Text_IO; use Ada.Text_IO; + + +with Tcp_Client; +with Backend_Thread; +with Webots_Thread; +with Types; use Types; +with Mailbox; +with Messages; use Messages; +with Byte_Buffer; +with Path_Following; +with collision_detection; + +procedure Main is + + task webots_task; + task backend_task; + + task body webots_task is + begin + Webots_Thread.Main; + end webots_task; + + task body backend_task is + begin + Backend_Thread.Main; + end backend_task; + + + Current_Mail : Mailbox.Mail; + alternator : Types.Uint8 := 1; + + DS_Data : Messages.Distance_Sensor_Array := (others => 1000.0); + LS_Data : Messages.Light_Sensor_Array := (others => 0.0); + V : Messages.Velocity_Message; + V_old : Messages.Velocity_Message; + -- D : Messages.Distance_Sensor_ Out_Buffer : Byte_Buffer.Buffer; + + --current_packet : types.Communication_Packet; + --send_packet_path_following : types.Communication_Packet; + --send_packet_collision_avoidance : types.Communication_Packet; + --dist: types.Octets_8; + -- distance_sensor_data: collision_detection.Dtype; + is_object_collision: Boolean := False; +begin + + V_old := Messages.Velocity_Message_Create (0.0, 0.0); + + -- threads have started here + while true loop + + -- clear out both mailboxes + Backend_Thread.Backend_Mailbox.Clear; + Webots_Thread.Webots_Mailbox.Clear; + + -- alternate between checking webots and backend mailbox first, then update alternator + Mailbox.check_mailbox (Backend_Thread.Backend_Mailbox, Webots_Thread.Webots_Mailbox, Current_Mail, alternator); + Mailbox.update_alternator (alternator); + + -- Put_Line (Current_Mail.Message.Id'Image); + + -- do calculations with current packet +-- <<<<<<< HEAD +-- --Ada.Text_IO.Put_Line(Integer'Image(Integer(current_packet.package_ID))); +-- +-- send_packet_path_following.payload_length := 0; +-- -- Path following +-- if current_packet.package_ID = 67 then +-- send_packet_path_following := pathfollowing.path_following(current_packet); +-- end if; +-- +-- -- Object collision +-- if current_packet.package_ID = 66 then +-- for J in uint32 range 0..8 loop +-- for I in uint32 range 0..7 loop +-- dist(I) := current_packet.local_payload(I+J*8); +-- end loop; +-- distance_sensor_data(Integer(J)) := Types.uint64_to_float64(octets_to_uint64(dist)); +-- end loop; +-- send_packet_collision_avoidance := detect(distance_sensor_data); +-- end if; +-- if send_packet_collision_avoidance.payload_length = 0 then +-- if send_packet_path_following.payload_length /= 0 then +-- send_bytes(Webots_Channel, send_packet_path_following); +-- end if; +-- else +-- send_bytes(Webots_Channel, send_packet_collision_avoidance); +-- ======= + if Current_Mail.Message.Id = Messages.EXTERNAL_IMAGE_DATA then + V := Path_Following.Main (Messages.ID_Message_Ptr (Current_Mail.Message), DS_Data); + --Out_Buffer.Write_Message (V); + --if (V_old.Left_Speed = V.Left_Speed) and (V_old.Right_Speed = V.Right_Speed) then + --if alternator = 1 then + -- V.Left_Speed := V.Left_Speed + 0.1; + -- V.Right_Speed := V.Right_Speed + 0.1; + -- else + -- V.Left_Speed := V.Left_Speed - 0.1; + -- V.Right_Speed := V.Right_Speed - 0.1; + -- end if; + -- end if; + -- V_old := V; + declare Out_Buffer : Byte_Buffer.Buffer; + begin + Out_Buffer.Write_Message(V); + Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer); + end; + elsif Current_Mail.Message.Id = Messages.EXTERNAL_DISTANCE_SENSOR then + DS_Data := Messages.DS_Message_Ptr (Current_Mail.Message).Payload; + --elsif Current_Mail.Message.Id = Messages.EXTERNAL_LIGHT_SENSOR then + --LS_Data := Messages.LS_Message_Ptr (Current_Mail.Message).Payload; + --Put_Line (LS_Data(0)'Image); + end if; + + -- Out_Buffer.Write_Message (V); + -- Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer); + -- Out_Buffer.Delete_Bytes (V.Size); + + + end loop; + +end Main; diff --git a/external/controller/src/Path/.#pathfollowing.adb# b/external/controller/src/Path/.#pathfollowing.adb# new file mode 100644 index 0000000..0ce294c --- /dev/null +++ b/external/controller/src/Path/.#pathfollowing.adb# @@ -0,0 +1,213 @@ +package body pathfollowing is + colour : Integer := 0; + r : Integer := 0; + g : Integer := 0; + b : Integer := 0; + + bottomPoint : Integer := 0; + topPoint : Integer := 0; + leftPoint : Integer := 0; + rightPoint : Integer:= 0; + bottomPoint1 : Integer := 0; + topPoint1 : Integer := 0; + leftPoint1 : Integer := 0; + rightPoint1 : Integer:= 0; + --steeringAngle : float64 := 0.0; + + red : Colour_Matrix := (others => (others => 0)); + blue : Colour_Matrix := (others => (others => 0)); + green : Colour_Matrix := (others => (others => 0)); + grey : Colour_Matrix := (others => (others => 0)); + binaImage : Colour_Matrix := (others => (others => 0)); + + wheehlvelocity : Wheehl_velocity := (others => 0.0); + --axleTrack : float64 := 1.1; + basicVelocity : float64 :=4.0 ; + --ratio : float64 := 8.0; + V_turn : float64 := 0.0; + offset : Integer := 32; + Kp : float64 := 0.08; + Ki : float64 :=0.08; + Kd : float64 := 0.08; + Error : float64 := 0.0; + lastError : float64 := 0.0; + integral : float64 := 0.0; + derivative : float64 := 0.0; + + + function path_following (imageInput : in Communication_Packet) return Communication_Packet is + raw : access types.payload := imageInput.local_payload; + index : Image_Index := 0; + o8 : Octets_8; + u64 : uint64; + begin + + for I in Row_Index loop + for J in Column_Index loop + index := Image_Index(4 * (Integer(I) * width + Integer(J))); + blue (I) (J) := raw (types.uint32(index) + 4); + green (I) (J) := raw (types.uint32(index + 1) + 4); + red (I) (J) := raw (types.uint32(index + 2) + 4); + colour := Integer(blue (I) (J))*114/1000 + Integer(green (I) (J)) *587/1000 + Integer(red (I) (J))*299/1000; + grey (I) (J) := uint8(colour); + end loop; + end loop; + + --Binarized + for I in Row_Index loop + for J in Column_Index loop + if grey (I)(J) > 100 then + binaImage (I)(J) := 255; + else + binaImage (I)(j) := 0; + end if; + --Put(uint8'Image (binaImage (I) (J))); + + end loop; + --Put_Line(""); + end loop; + + --for I in Row_Index loop + --for J in Column_Index loop + --Put(Value'Image (blue (I) (J))); + --Put(Value'Image (green (I) (J))); + --Put(Value'Image (red (I) (J))); + --Put(Value'Image (grey (I) (J))); + --Put_Line(""); + --end loop; + --end loop; +-- find bottompoint + for J in Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint := Integer(J); + end if; + -- + end loop; + for J in reverse Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint1 := Integer(J); + end if; + end loop; + bottomPoint := (bottomPoint + bottomPoint1) / 2; + -- find toppoint + for J in Column_Index loop + if binaImage(0)(J) = 255 then + topPoint := Integer(J); + end if; + end loop; + for J in reverse Column_Index loop + if binaImage(Row_Index(0))(J) = 255 then + topPoint1 := Integer(J); + end if; + end loop; + topPoint := (topPoint + topPoint1) / 2; + -- find rightpoint + for I in Row_Index loop + if binaImage(I)(Column_Index(width-1)) = 255 then + rightPoint:= Integer(I); + end if; + end loop; + for I in reverse Row_Index loop + if binaImage(I)(Column_Index(width-1)) = 255 then + rightPoint1:= Integer(I); + end if; + end loop; + rightPoint := (rightPoint + rightPoint1) /2; + + --find leftPoint + for I in Row_Index loop + if binaImage(I)(0) = 255 then + leftPoint:= Integer(I); + end if; + end loop; + for I in reverse Row_Index loop + if binaImage(I)(0) = 255 then + leftPoint1 := Integer(I); + end if; + end loop; + leftPoint := (leftPoint + leftPoint1)/2; + + Put_Line(Integer'Image(bottomPoint) & Integer'Image(topPoint) & Integer'Image(rightPoint) & Integer'Image(leftPoint)); + --calculate steering angle + -- top and bottom + --if bottomPoint /= 0 and topPoint /= 0 and rightPoint = 0 and leftPoint = 0 then + --if topPoint - bottomPoint > 0 then + --steeringAngle := float64((topPoint - bottomPoint) / height) ; + --elsif topPoint - bottomPoint < 0 then + --steeringAngle := -float64((topPoint - bottomPoint) / height); + --else steeringAngle := 0.0; + --end if; + --end if; + -- bottom and left + --if bottomPoint /= 0 and leftPoint/= 0 and topPoint = 0 and rightPoint = 0 then + --steeringAngle := - float64(bottomPoint / (height - leftPoint)); + --end if; + -- top and right + --if topPoint /= 0 and rightPoint /= 0 and bottomPoint = 0 and leftPoint= 0 then + --steeringAngle := - float64((width - topPoint) / rightPoint); + --end if; + --top and left + --if topPoint /= 0 and leftPoint /=0 and bottomPoint = 0 and rightPoint = 0 then + --steeringAngle := float64(topPoint / leftPoint); + --end if; + -- bottom and right + --if bottomPoint/= 0 and rightPoint /= 0 and topPoint =0 and leftPoint = 0 then + --steeringAngle := float64((width-bottomPoint) / (height - leftPoint)); + --end if; + --Error : = bottomPoint - offset; + --integral : = integral + Error; + --derivative : = Error - lastError; + --V_turn := Kp * Error + Ki * integral + Kd * derivative; + + + + if bottomPoint >= 36 then + V_turn := 3.6; + elsif bottomPoint <= 28 and bottomPoint > 0 then + V_turn := -3.6; + else V_turn := 0.0; + end if; + + + Put_Line (V_turn'Image); + --turn right + + if V_turn > 0.0 then + wheehlvelocity(0) := basicVelocity + V_turn; + wheehlvelocity(1) := basicVelocity - V_turn; + --turn left + elsif V_turn < 0.0 then + wheehlvelocity(0) := basicVelocity + V_turn; + wheehlvelocity(1) := basicVelocity - V_turn; + else + wheehlvelocity(0) := basicVelocity; + wheehlvelocity(1) := basicVelocity; + end if; + + Put_Line (wheehlvelocity (0)'Image & ", " & wheehlvelocity (1)'Image); + + declare packet : Communication_Packet; + begin + packet.package_ID := 129; + packet.payload_length := 16 + 5; + packet.local_payload := new payload (0 .. 15); + + u64 := types.float64_to_uint64 (wheehlvelocity (0)); + o8 := types.uint64_to_octets (u64); + + for I in o8'Range loop + packet.local_payload (I) := o8 (I); + end loop; + + u64 := types.float64_to_uint64 (wheehlvelocity (1)); + o8 := types.uint64_to_octets (u64); + + for I in o8'Range loop + packet.local_payload (I + 8) := o8 (I); + end loop; + + return packet; + end; + end path_following; + +end pathfollowing; diff --git a/external/controller/src/Path/pathfollowing.adb b/external/controller/src/Path/pathfollowing.adb new file mode 100644 index 0000000..5e0eda9 --- /dev/null +++ b/external/controller/src/Path/pathfollowing.adb @@ -0,0 +1,212 @@ +package body pathfollowing is + colour : Integer := 0; + r : Integer := 0; + g : Integer := 0; + b : Integer := 0; + + bottomPoint : Integer := 0; + topPoint : Integer := 0; + leftPoint : Integer := 0; + rightPoint : Integer:= 0; + bottomPoint1 : Integer := 0; + topPoint1 : Integer := 0; + leftPoint1 : Integer := 0; + rightPoint1 : Integer:= 0; + --steeringAngle : float64 := 0.0; + + red : Colour_Matrix := (others => (others => 0)); + blue : Colour_Matrix := (others => (others => 0)); + green : Colour_Matrix := (others => (others => 0)); + grey : Colour_Matrix := (others => (others => 0)); + binaImage : Colour_Matrix := (others => (others => 0)); + + wheehlvelocity : Wheehl_velocity := (others => 0.0); + --axleTrack : float64 := 1.1; + basicVelocity : float64 :=2.0 ; + --ratio : float64 := 8.0; + V_turn : float64 := 0.0; + offset : Integer := 32; + Kp : float64 := 0.08; + Ki : float64 :=0.08; + Kd : float64 := 0.08; + Error : float64 := 0.0; + lastError : float64 := 0.0; + integral : float64 := 0.0; + derivative : float64 := 0.0; + + + function path_following (imageInput : in Communication_Packet) return Communication_Packet is + raw : access types.payload := imageInput.local_payload; + index : Image_Index := 0; + o8 : Octets_8; + u64 : uint64; + begin + for I in Row_Index loop + for J in Column_Index loop + index := Image_Index(4 * (Integer(I) * width + Integer(J))); + blue (I) (J) := raw (types.uint32(index) + 4); + green (I) (J) := raw (types.uint32(index + 1) + 4); + red (I) (J) := raw (types.uint32(index + 2) + 4); + colour := Integer(blue (I) (J))*114/1000 + Integer(green (I) (J)) *587/1000 + Integer(red (I) (J))*299/1000; + grey (I) (J) := uint8(colour); + end loop; + end loop; + + --Binarized + for I in Row_Index loop + for J in Column_Index loop + if grey (I)(J) > 100 then + binaImage (I)(J) := 255; + else + binaImage (I)(j) := 0; + end if; + --Put(uint8'Image (binaImage (I) (J))); + + end loop; + --Put_Line(""); + end loop; + + --for I in Row_Index loop + --for J in Column_Index loop + --Put(Value'Image (blue (I) (J))); + --Put(Value'Image (green (I) (J))); + --Put(Value'Image (red (I) (J))); + --Put(Value'Image (grey (I) (J))); + --Put_Line(""); + --end loop; + --end loop; +-- find bottompoint + for J in Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint := Integer(J); + end if; + -- + end loop; + for J in reverse Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint1 := Integer(J); + end if; + end loop; + bottomPoint := (bottomPoint + bottomPoint1) / 2; + -- find toppoint + for J in Column_Index loop + if binaImage(0)(J) = 255 then + topPoint := Integer(J); + end if; + end loop; + for J in reverse Column_Index loop + if binaImage(Row_Index(0))(J) = 255 then + topPoint1 := Integer(J); + end if; + end loop; + topPoint := (topPoint + topPoint1) / 2; + -- find rightpoint + for I in Row_Index loop + if binaImage(I)(Column_Index(width-1)) = 255 then + rightPoint:= Integer(I); + end if; + end loop; + for I in reverse Row_Index loop + if binaImage(I)(Column_Index(width-1)) = 255 then + rightPoint1:= Integer(I); + end if; + end loop; + rightPoint := (rightPoint + rightPoint1) /2; + + --find leftPoint + for I in Row_Index loop + if binaImage(I)(0) = 255 then + leftPoint:= Integer(I); + end if; + end loop; + for I in reverse Row_Index loop + if binaImage(I)(0) = 255 then + leftPoint1 := Integer(I); + end if; + end loop; + leftPoint := (leftPoint + leftPoint1)/2; + + -- Put_Line(Integer'Image(bottomPoint) & Integer'Image(topPoint) & Integer'Image(rightPoint) & Integer'Image(leftPoint)); + --calculate steering angle + -- top and bottom + --if bottomPoint /= 0 and topPoint /= 0 and rightPoint = 0 and leftPoint = 0 then + --if topPoint - bottomPoint > 0 then + --steeringAngle := float64((topPoint - bottomPoint) / height) ; + --elsif topPoint - bottomPoint < 0 then + --steeringAngle := -float64((topPoint - bottomPoint) / height); + --else steeringAngle := 0.0; + --end if; + --end if; + -- bottom and left + --if bottomPoint /= 0 and leftPoint/= 0 and topPoint = 0 and rightPoint = 0 then + --steeringAngle := - float64(bottomPoint / (height - leftPoint)); + --end if; + -- top and right + --if topPoint /= 0 and rightPoint /= 0 and bottomPoint = 0 and leftPoint= 0 then + --steeringAngle := - float64((width - topPoint) / rightPoint); + --end if; + --top and left + --if topPoint /= 0 and leftPoint /=0 and bottomPoint = 0 and rightPoint = 0 then + --steeringAngle := float64(topPoint / leftPoint); + --end if; + -- bottom and right + --if bottomPoint/= 0 and rightPoint /= 0 and topPoint =0 and leftPoint = 0 then + --steeringAngle := float64((width-bottomPoint) / (height - leftPoint)); + --end if; + --Error : = bottomPoint - offset; + --integral : = integral + Error; + --derivative : = Error - lastError; + --V_turn := Kp * Error + Ki * integral + Kd * derivative; + + + + if bottomPoint >= 36 then + V_turn := 0.9; + elsif bottomPoint <= 28 and bottomPoint > 0 then + V_turn := -0.9; + else V_turn := 0.0; + end if; + + + -- Put_Line (V_turn'Image); + --turn right + + if V_turn > 0.0 then + wheehlvelocity(0) := basicVelocity + V_turn; + wheehlvelocity(1) := basicVelocity - V_turn; + --turn left + elsif V_turn < 0.0 then + wheehlvelocity(0) := basicVelocity + V_turn; + wheehlvelocity(1) := basicVelocity - V_turn; + else + wheehlvelocity(0) := basicVelocity; + wheehlvelocity(1) := basicVelocity; + end if; + +-- Put_Line (wheehlvelocity (0)'Image & ", " & wheehlvelocity (1)'Image); + + declare packet : Communication_Packet; + begin + packet.package_ID := 129; + packet.payload_length := 16 + 5; + packet.local_payload := new payload (0 .. 15); + + u64 := types.float64_to_uint64 (wheehlvelocity (0)); + o8 := types.uint64_to_octets (u64); + + for I in o8'Range loop + packet.local_payload (I) := o8 (I); + end loop; + + u64 := types.float64_to_uint64 (wheehlvelocity (1)); + o8 := types.uint64_to_octets (u64); + + for I in o8'Range loop + packet.local_payload (I + 8) := o8 (I); + end loop; + + return packet; + end; + end path_following; + +end pathfollowing; diff --git a/external/controller/src/Path/pathfollowing.ads b/external/controller/src/Path/pathfollowing.ads new file mode 100644 index 0000000..9ff2a15 --- /dev/null +++ b/external/controller/src/Path/pathfollowing.ads @@ -0,0 +1,26 @@ +with Ada.Text_IO; use Ada.Text_IO; +with Ada.Float_Text_Io; use Ada.Float_Text_Io; +with types; use types; +with mailbox; + +package pathfollowing is + + width : constant Integer := 64; + height : constant Integer := 64; + size : constant Integer := width * height * 4; + + type Pixel is array(0 .. 3) of uint8; + type Column_Index is range 0 .. width - 1; + type Row_Index is range 0 .. height - 1; + + type Image_Index is range 0 .. size - 1; + type Image_Raw is array(Image_Index) of uint8; + + type Colour_Column is array(Column_Index) of uint8; + type Colour_Matrix is array(Row_Index) of Colour_Column; + type Wheehl_velocity is array(0..1) of float64; + type Dtype is array(0..8) of float64; + + function path_following(imageInput : in Communication_Packet; d_sensor : in Dtype) return Communication_Packet; + +end pathfollowing; diff --git a/external/controller/src/collision_detection/.#object_collision.adb# b/external/controller/src/collision_detection/.#object_collision.adb# new file mode 100644 index 0000000..dd3280d --- /dev/null +++ b/external/controller/src/collision_detection/.#object_collision.adb# @@ -0,0 +1,62 @@ +with Ada.Text_IO; +with ADA.Integer_Text_IO; + +package body object_collision is + + procedure collision_detection_main is + distance: Dtype; + begin + car_state := Forward; + while True loop + -- Get distances + Ada.Text_IO.Put_Line("Enter distances:"); + for I in 0..Dtype'Length-1 loop + Ada.Integer_Text_IO.Get(distance(I)); + end loop; + detect(distance); + end loop; + + end ; + + procedure detect(distance: dtype) is + threshold: Integer := 10; + begin + -- Object in front of car + case car_state is + when Forward => + if distance(1) < threshold then + -- Slow down car + Ada.Text_IO.Put_Line("Slow down or stop car"); + + -- Take on obstacle + if distance(2) > threshold then + Ada.Text_IO.Put_Line("Take on right side"); + car_state := Right; + elsif distance(0) > threshold then + Ada.Text_IO.Put_Line("Take on left side"); + car_state := Left; + else + Ada.Text_IO.Put_Line("There is no space"); + end if; + end if; + + when Right => + Ada.Text_IO.Put_Line("Right"); + if distance(0) > threshold then + Ada.Text_IO.Put_Line("Get Back to path"); + car_state := Forward; + end if; + + when Left => + if distance(2) > threshold then + Ada.Text_IO.Put_Line("Get Back to path"); + car_state := Forward; + end if; + + end case; + + + end; + + +end ; diff --git a/external/controller/src/collision_detection/collision_detection.adb b/external/controller/src/collision_detection/collision_detection.adb new file mode 100644 index 0000000..94eec87 --- /dev/null +++ b/external/controller/src/collision_detection/collision_detection.adb @@ -0,0 +1,120 @@ +with Ada.Text_IO; +with ADA.Integer_Text_IO; +with tcp_client; use tcp_client; +with webots_thread; use webots_thread; +with Ada.Float_Text_IO; + +package body collision_detection is + + function detect(distance: Dtype) return Communication_Packet is + threshold: float64 := 900.0; + u64 : types.uint64; + o8 : types.Octets_8; + moves_left: Integer; + begin + -- Object in front of car + case car_state is + when Forward => + -- Ada.Float_Text_IO.Put(FLoat(distance(0)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(1)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(2)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(3)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(4)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(5)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(6)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(7)),5,3,0); +-- Ada.Float_Text_IO.Put(FLoat(distance(8)),5,3,0); +-- Ada.Text_IO.Put_Line(""); + if distance(0) < threshold then + -- Slow down car + + ls := -10.0; + + rs := 10.0; + + Ada.Text_IO.Put_Line("Slow down or stop car"); + -- Take on obstacle + if distance(2) > threshold then + Ada.Text_IO.Put_Line("Take on right side"); + moves_left := 0; + car_state := Left; + elsif distance(7) > threshold then + Ada.Text_IO.Put_Line("Take on left side"); + car_state := Right; + else + Ada.Text_IO.Put_Line("There is no space"); + end if; + end if; + + when Left => + moves_left := moves_left + 1; + if distance(0) > threshold then + + ls := 1.5; + + rs := 1.5; + + + end if; + + when Get_back_Left=> +-- +-- if distance(3) > 500.0 then +-- Webots_Cmd.package_ID := 129; +-- Webots_Cmd.payload_length := 5 + 16; +-- Webots_Cmd.local_payload := new types.payload (0 .. 15); +-- ls := 10.0; +-- u64 := types.float64_to_uint64 (ls); +-- o8 := types.uint64_to_octets (u64); +-- for I in o8'Range loop +-- Webots_Cmd.local_payload (I) := o8 (I); +-- end loop; +-- rs := - 10.0; +-- u64 := types.float64_to_uint64 (rs); +-- o8 := types.uint64_to_octets (u64); +-- for I in o8'Range loop +-- Webots_Cmd.local_payload (I + 8) := o8 (I); +-- end loop; + ADA.Integer_Text_IO.Put(Integer(Webots_Cmd.package_ID)); +-- send_bytes(Webots_Channel, Webots_Cmd); +-- + + when Right => + Ada.Text_IO.Put("Left"); + if distance(2) > threshold then + Ada.Text_IO.Put_Line("Get Back to path"); + car_state := Forward; + end if; + + end case; + if ls = 0.0 and rs = 0.0 then + declare packet : Communication_Packet; + begin + packet.payload_length := 0; + return packet; + end; + else + declare packet : Communication_Packet; + begin + packet.package_ID := 129; + packet.payload_length := 5 + 16; + packet.local_payload := new types.payload (0 .. 15); + u64 := types.float64_to_uint64 (ls); + o8 := types.uint64_to_octets (u64); + for I in o8'Range loop + packet.local_payload (I) := o8 (I); + end loop; + u64 := types.float64_to_uint64 (rs); + o8 := types.uint64_to_octets (u64); + for I in o8'Range loop + packet.local_payload (I + 8) := o8 (I); + end loop; + + return packet; + end; + end if; + + + end; + +end ; diff --git a/external/controller/src/collision_detection/collision_detection.ads b/external/controller/src/collision_detection/collision_detection.ads new file mode 100644 index 0000000..4db20b5 --- /dev/null +++ b/external/controller/src/collision_detection/collision_detection.ads @@ -0,0 +1,11 @@ +with types;use types; + +package collision_detection is + + type State is (Forward, Right, Left, Get_back_Left); + type Dtype is array(0..8) of float64; + car_state: State; + function detect(distance: Dtype) return types.Communication_Packet; + ls, rs : types.float64 := 0.0; + +end collision_detection; diff --git a/external/controller/src/main.adb b/external/controller/src/main.adb index 026dd6c..245f824 100644 --- a/external/controller/src/main.adb +++ b/external/controller/src/main.adb @@ -1,13 +1,15 @@ with Ada.Text_IO; use Ada.Text_IO; + with Tcp_Client; with Backend_Thread; with Webots_Thread; -with Types; +with Types; use Types; with Mailbox; -with Messages; +with Messages; use Messages; with Byte_Buffer; - +with Path_Following; +with collision_detection; procedure Main is @@ -24,16 +26,27 @@ procedure Main is Backend_Thread.Main; end backend_task; + Current_Mail : Mailbox.Mail; alternator : Types.Uint8 := 1; + DS_Data : Messages.Distance_Sensor_Array := (others => 1000.0); + LS_Data : Messages.Light_Sensor_Array := (others => 0.0); V : Messages.Velocity_Message; - Out_Buffer : Byte_Buffer.Buffer; - + V_old : Messages.Velocity_Message; + -- D : Messages.Distance_Sensor_ Out_Buffer : Byte_Buffer.Buffer; + + --current_packet : types.Communication_Packet; + --send_packet_path_following : types.Communication_Packet; + --send_packet_collision_avoidance : types.Communication_Packet; + --dist: types.Octets_8; + -- distance_sensor_data: collision_detection.Dtype; + is_object_collision: Boolean := False; begin - -- threads have started here + V_old := Messages.Velocity_Message_Create (0.0, 0.0); + -- threads have started here while true loop -- clear out both mailboxes @@ -44,14 +57,63 @@ begin Mailbox.check_mailbox (Backend_Thread.Backend_Mailbox, Webots_Thread.Webots_Mailbox, Current_Mail, alternator); Mailbox.update_alternator (alternator); - -- do calculations with current packet - Put_Line (Current_Mail.Message.Id'Image); - - V := Messages.Velocity_Message_Create (5.1, 2.4); + -- Put_Line (Current_Mail.Message.Id'Image); - Out_Buffer.Write_Message (V); - Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer); - Out_Buffer.Delete_Bytes (V.Size); + -- do calculations with current packet +-- <<<<<<< HEAD +-- --Ada.Text_IO.Put_Line(Integer'Image(Integer(current_packet.package_ID))); +-- +-- send_packet_path_following.payload_length := 0; +-- -- Path following +-- if current_packet.package_ID = 67 then +-- send_packet_path_following := pathfollowing.path_following(current_packet); +-- end if; +-- +-- -- Object collision +-- if current_packet.package_ID = 66 then +-- for J in uint32 range 0..8 loop +-- for I in uint32 range 0..7 loop +-- dist(I) := current_packet.local_payload(I+J*8); +-- end loop; +-- distance_sensor_data(Integer(J)) := Types.uint64_to_float64(octets_to_uint64(dist)); +-- end loop; +-- send_packet_collision_avoidance := detect(distance_sensor_data); +-- end if; +-- if send_packet_collision_avoidance.payload_length = 0 then +-- if send_packet_path_following.payload_length /= 0 then +-- send_bytes(Webots_Channel, send_packet_path_following); +-- end if; +-- else +-- send_bytes(Webots_Channel, send_packet_collision_avoidance); +-- ======= + if Current_Mail.Message.Id = Messages.EXTERNAL_IMAGE_DATA then + V := Path_Following.Main (Messages.ID_Message_Ptr (Current_Mail.Message), DS_Data); + --Out_Buffer.Write_Message (V); + --if (V_old.Left_Speed = V.Left_Speed) and (V_old.Right_Speed = V.Right_Speed) then + --if alternator = 1 then + -- V.Left_Speed := V.Left_Speed + 0.1; + -- V.Right_Speed := V.Right_Speed + 0.1; + -- else + -- V.Left_Speed := V.Left_Speed - 0.1; + -- V.Right_Speed := V.Right_Speed - 0.1; + -- end if; + -- end if; + -- V_old := V; + declare Out_Buffer : Byte_Buffer.Buffer; + begin + Out_Buffer.Write_Message(V); + Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer); + end; + elsif Current_Mail.Message.Id = Messages.EXTERNAL_DISTANCE_SENSOR then + DS_Data := Messages.DS_Message_Ptr (Current_Mail.Message).Payload; + elsif Current_Mail.Message.Id = Messages.EXTERNAL_LIGHT_SENSOR then + LS_Data := Messages.LS_Message_Ptr (Current_Mail.Message).Payload; + Put_Line (LS_Data(0)'Image); + end if; + + -- Out_Buffer.Write_Message (V); + -- Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer); + -- Out_Buffer.Delete_Bytes (V.Size); end loop; diff --git a/external/controller/src/path/path_following.adb b/external/controller/src/path/path_following.adb new file mode 100644 index 0000000..8cd04d0 --- /dev/null +++ b/external/controller/src/path/path_following.adb @@ -0,0 +1,257 @@ +package body Path_Following is + colour : Integer := 0; + r : Integer := 0; + g : Integer := 0; + b : Integer := 0; + + bottomPoint : Integer := 0; + --topPoint : Integer := 0; + --leftPoint : Integer := 0; + --rightPoint : Integer:= 0; + bottomPoint1 : Integer := 0; + white_Line : Integer := 0; + --topPoint1 : Integer := 0; + --leftPoint1 : Integer := 0; + --rightPoint1 : Integer:= 0; + --steeringAngle : float64 := 0.0; + + red : Colour_Matrix := (others => (others => 0)); + blue : Colour_Matrix := (others => (others => 0)); + green : Colour_Matrix := (others => (others => 0)); + grey : Colour_Matrix := (others => (others => 0)); + binaImage : Colour_Matrix := (others => (others => 0)); + + wheehlvelocity : Velocity_Array := (others => 0.0); + --axleTrack : float64 := 1.1; + basicVelocity : Float64 :=4.0 ; + --ratio : float64 := 8.0; + V_turn : Float64 := 0.0; + --offset : Integer := 32; + --Kp : float64 := 0.08; + --Ki : float64 :=0.08; + --Kd : float64 := 0.08; + --Error : float64 := 0.0; + --lastError : float64 := 0.0; + --integral : float64 := 0.0; + --derivative : float64 := 0.0; + + function Binarize (grey : in Colour_Matrix) return Colour_Matrix is + begin + + for I in Row_Index loop + for J in Column_Index loop + if grey (I)(J) > 150 then + binaImage (I)(J) := 255; + else + binaImage (I)(j) := 0; + end if; + --Put(uint8'Image (binaImage (I) (J))); + + end loop; + --Put_Line(""); + end loop; + return binaImage; + + end Binarize; + + function Find_Line (binarizedImage : in Colour_Matrix) return Integer is + begin + for J in Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint := Integer(J); + end if; + -- + end loop; + + for J in reverse Column_Index loop + if binaImage(Row_Index(height-5))(J) = 255 then + bottomPoint1 := Integer(J); + end if; + end loop; + + bottomPoint := (bottomPoint + bottomPoint1) / 2; + Put_Line(Integer'Image(bottomPoint)); + return bottomPoint; + + end Find_Line; + + procedure Check_For_Fork (pick_up_location_reached : in Boolean; bottomPoint : in out Integer; bottomPoint1 : in out Integer ) is + + old_value : Integer; + + begin + + -- current assumption: bottomPoint: right, bottomPoint1: left + -- careful: bottomPoint might be right point and bottomPoint1 might be the left point + + -- save old value + if pick_up_location_reached = True then + old_value := bottomPoint1; + -- check right side + for J in reverse Integer range bottomPoint1..bottomPoint loop + --Put_Line(Integer'Image(J) & ": " & Integer'Image(Integer(binaImage(Row_Index(height-5))(Column_Index(J))))); + -- if black pixel found, update bottomPoint + if binaImage(Row_Index(height-5))(Column_Index(J)) /= 255 then + bottomPoint1 := Integer(J) + 1; + --Put_Line("Fork detected."); + --Put_Line("Trying to go right."); + --Put_Line("old_value: " & Integer'Image(old_value)); + --Put_Line("new bottomPoint1: " & Integer'Image(bottomPoint1)); + end if; + + -- if the value changed then exit + exit when old_value /= bottomPoint1; + end loop; + else + + old_value := bottomPoint; + -- check left side + for J in Integer range bottomPoint1..bottomPoint loop + + -- if black pixel found, update bottomPoint1 + if binaImage(Row_Index(height-5))(Column_Index(J)) /= 255 then + bottomPoint := Integer(J) - 1; + --Put_Line("Fork detected."); + --Put_Line("Trying to go left."); + --Put_Line("old_value: " & Integer'Image(old_value)); + --Put_Line("new bottomPoint1: " & Integer'Image(bottomPoint)); + end if; + + -- if the value changed then exit + exit when old_value /= bottomPoint; + end loop; + end if; + + end Check_For_Fork; + + + function Wheel_Velocity (whiteLine : in Integer; d_sensor : in Messages.Distance_Sensor_Array) return Velocity_Array is + Velocity : Velocity_Array := (others => 0.0); + + steering_zones_size : Long_Float := Long_Float(width)/13.0; + + type steering_zones is array(1..13) of Long_Float; + + steering_zones_array : steering_zones; + + begin + + for I in steering_zones_array'Range loop + steering_zones_array(I) := Long_Float(I)*steering_zones_size; + end loop; + + + if whiteLine = 0 then + if d_sensor (6) < 500.0 then + V_turn := 1.0; + end if; + if d_sensor (3) < 500.0 then + V_turn := -1.0; + end if; + else + if Long_Float(whiteLine) < steering_zones_array(1) then + V_turn := -2.0; + elsif Long_Float(whiteLine) < steering_zones_array(2) then + V_turn := -1.5; + elsif Long_Float(whiteLine) < steering_zones_array(3) then + V_turn := -1.25; + elsif Long_Float(whiteLine) < steering_zones_array(4) then + V_turn := -0.75; + elsif Long_Float(whiteLine) < steering_zones_array(5) then + V_turn := -0.5; + elsif Long_Float(whiteLine) < steering_zones_array(6) then + V_turn := -0.25; + elsif Long_Float(whiteLine) < steering_zones_array(7) then-- straight + V_turn := 0.0; + elsif Long_Float(whiteLine) < steering_zones_array(8) then + V_turn := 0.255; + elsif Long_Float(whiteLine) < steering_zones_array(9) then + V_turn := 0.5; + elsif Long_Float(whiteLine) < steering_zones_array(10) then + V_turn := 0.75; + elsif Long_Float(whiteLine) < steering_zones_array(11) then + V_turn := 1.25; + elsif Long_Float(whiteLine) < steering_zones_array(12) then + V_turn := 1.5; + else + V_turn := 2.0; + end if; + end if; + + + + --if whiteLine >= 36 then + --V_turn := 3.6; + --elsif whiteLine <= 28 and whiteLine > 0 then + --V_turn := -3.6; + -- elsif whiteLine > 28 and whiteLine < 36 then + -- V_turn := 0.0; + -- elsif whiteLine = 0 then + -- if d_sensor (6) < 500.0 then + -- V_turn := 1.0; + -- end if; + -- if d_sensor (3) < 500.0 then + -- V_turn := -1.0; + -- end if; + -- end if; + + --Put_Line (d_sensor(3)'Image & ", " & d_sensor(6)'Image); + --Put_Line (V_turn'Image); + --turn right + + if V_turn > 0.0 then + Velocity (0) := basicVelocity + V_turn; + Velocity (1) := basicVelocity - V_turn; + --turn left + elsif V_turn < 0.0 then + Velocity (0) := basicVelocity + V_turn; + Velocity (1) := basicVelocity - V_turn; + else + Velocity (0) := basicVelocity; + Velocity (1) := basicVelocity; + end if; + Put_Line (Velocity (0)'Image & ", " & Velocity (1)'Image); + + Put_Line ("front_ds: " & d_sensor(2)'Image); + + if d_sensor (2) < 300.0 then + Velocity (0) := 1.0; + Velocity (1) := -1.0; + end if; + + return Velocity; + + end Wheel_Velocity; + + function Main (Data_Input : in Messages.ID_Message_Ptr; d_sensor : in Messages.Distance_Sensor_Array) return Messages.Velocity_Message is + Index : Image_Index := 0; + Velocity : Velocity_Array; + begin + --grayscale Image + for I in Row_Index loop + for J in Column_Index loop + Index := Image_Index(4 * (Integer(I) * width + Integer(J))); + blue (I) (J) := Data_Input.Payload (Types.Uint32(Index)); + green (I) (J) := Data_Input.Payload (Types.Uint32(Index + 1)); + red (I) (J) := Data_Input.Payload (Types.Uint32(Index + 2)); + colour := Integer(blue (I) (J)) * 114/1000 + Integer(green (I) (J)) * 587/1000 + Integer(red (I) (J)) * 299/1000; + grey (I) (J) := uint8(colour); + end loop; + end loop; + + --Binarized + binaImage := Binarize (grey); + --findLine + white_Line := Find_Line (binaImage); + --wheelvelocity calculate + Velocity := Wheel_Velocity (white_Line, d_sensor); + + declare + Message : Messages.Velocity_Message := + Messages.Velocity_Message_Create (Velocity (0), Velocity (1)); + begin + return Message; + end; + end Main; + +end Path_Following; diff --git a/external/controller/src/path/path_following.ads b/external/controller/src/path/path_following.ads new file mode 100644 index 0000000..dff344b --- /dev/null +++ b/external/controller/src/path/path_following.ads @@ -0,0 +1,33 @@ +with Ada.Text_IO; use Ada.Text_IO; +with Ada.Float_Text_Io; use Ada.Float_Text_Io; + +with Types; use Types; +with Mailbox; +with Messages; + +package Path_Following is + + width : constant Integer := 64; + --height : constant Integer := 64; + height : constant Integer := 5; + size : constant Integer := width * height * 4; + + type Pixel is array(0 .. 3) of uint8; + type Column_Index is range 0 .. width - 1; + type Row_Index is range 0 .. height - 1; + + type Image_Index is range 0 .. size - 1; + type Image_Raw is array(Image_Index) of uint8; + + type Colour_Column is array(Column_Index) of uint8; + type Colour_Matrix is array(Row_Index) of Colour_Column; + type Velocity_Array is array(0..1) of Float64; + + function Main (Data_Input : in Messages.ID_Message_Ptr; d_sensor : in Messages.Distance_Sensor_Array) return Messages.Velocity_Message; + function Binarize (grey : in Colour_Matrix) return Colour_Matrix; + function Find_Line (binarizedImage : in Colour_Matrix) return Integer; + function Wheel_Velocity (whiteLine : in Integer; d_sensor : in Messages.Distance_Sensor_Array) return Velocity_Array; + + procedure Check_For_Fork (pick_up_location_reached : in Boolean; bottomPoint : in out Integer; bottomPoint1 : in out Integer ); + +end Path_Following; diff --git a/external/controller/src/tcp_client/byte_buffer.adb b/external/controller/src/tcp_client/byte_buffer.adb index 99d65dc..5c0eee5 100644 --- a/external/controller/src/tcp_client/byte_buffer.adb +++ b/external/controller/src/tcp_client/byte_buffer.adb @@ -280,7 +280,8 @@ package body Byte_Buffer is procedure Write_Stream (Stream : not null access Ada.Streams.Root_Stream_Type'Class; In_Buffer : in Buffer) is B : Buffer := In_Buffer; - Stream_Buffer : Ada.Streams.Stream_Element_Array (0 .. Ada.Streams.Stream_Element_Offset (B.Bytes_Remaining - 1)); + Stream_Buffer : Ada.Streams.Stream_Element_Array + (0 .. Ada.Streams.Stream_Element_Offset (B.Bytes_Remaining - 1)); begin for I in Stream_Buffer'Range loop B.Read_Uint8 (Types.Uint8 (Stream_Buffer (I))); @@ -305,7 +306,8 @@ package body Byte_Buffer is Size := Types.Octets_To_Uint32 (O4); declare - Stream_Buffer2 : Ada.Streams.Stream_Element_Array (0 .. Ada.Streams.Stream_Element_Offset (Size - 5)); + Stream_Buffer2 : Ada.Streams.Stream_Element_Array + (0 .. Ada.Streams.Stream_Element_Offset (Size - 5)); begin Stream.Read (Stream_Buffer2, Last); diff --git a/external/controller/src/tcp_client/mailbox.adb b/external/controller/src/tcp_client/mailbox.adb index bf544b2..02f52fe 100644 --- a/external/controller/src/tcp_client/mailbox.adb +++ b/external/controller/src/tcp_client/mailbox.adb @@ -44,15 +44,15 @@ package body Mailbox is select first.Collect (new_packet); else - delay(0.05); - Check_Mailbox (second, first, new_packet, alternator); + delay(0.00005); + check_mailbox(second,first,new_packet,alternator); end select; else select second.Collect (new_packet); else - delay(0.05); - Check_Mailbox (second, first, new_packet, alternator); + delay(0.00005); + check_mailbox(second,first,new_packet,alternator); end select; end if; end Check_Mailbox; diff --git a/external/controller/src/tcp_client/messages.ads b/external/controller/src/tcp_client/messages.ads index e94a9f6..7629413 100644 --- a/external/controller/src/tcp_client/messages.ads +++ b/external/controller/src/tcp_client/messages.ads @@ -28,6 +28,7 @@ package Messages is type Distance_Sensor_Array is array(0 .. 8) of Types.Float64; + -- interesting ones: 2: front ,3,6,5,8 type Distance_Sensor_Message is new Message with record Payload : Distance_Sensor_Array; end record; diff --git a/external/controller/src/tcp_client/tcp_client.adb b/external/controller/src/tcp_client/tcp_client.adb index 00d41f2..666e17d 100644 --- a/external/controller/src/tcp_client/tcp_client.adb +++ b/external/controller/src/tcp_client/tcp_client.adb @@ -19,31 +19,6 @@ package body Tcp_Client is end Connect; - - procedure Send_Bytes (server_stream : Stream_Access; outgoing_packet : Types.Communication_Packet) is - - uint8_payload_length : Types.Octets_4; - - begin - - -- send payload_length - uint8_payload_length := Types.Uint32_To_Octets(outgoing_packet.payload_length); - for I in uint8_payload_length'Range loop - Types.Uint8'Write(server_stream, uint8_payload_length(I)); - end loop; - - -- send package_ID - Types.Uint8'Write(server_stream, outgoing_packet.package_ID); - - -- write full payload to stream - if outgoing_packet.payload_length > 0 then - for I in outgoing_packet.local_payload'Range loop - Types.Uint8'Write(server_stream, outgoing_packet.local_payload(I)); - end loop; - end if; - - end Send_Bytes; - procedure Read_Packet (server_stream : Stream_Access; dynamic_buffer : in out Byte_Buffer.Buffer; local_mailbox : in out Mailbox.Mailbox) is diff --git a/external/controller/src/tcp_client/tcp_client.ads b/external/controller/src/tcp_client/tcp_client.ads index 9768549..ebde23f 100644 --- a/external/controller/src/tcp_client/tcp_client.ads +++ b/external/controller/src/tcp_client/tcp_client.ads @@ -15,14 +15,8 @@ package Tcp_Client is protocol_ID_length : Types.Uint32 := 1; protocol_join_ID : Types.Uint8 := 6; - function Connect (client : in out Socket_Type; port : Port_Type; address : in out Sock_Addr_Type) return Stream_Access; --builds the connection and then return the socket - - --procedure Send_Bytes (server_stream : Stream_Access; outgoing_packet : Types.Communication_Packet); --sends a bytes over stream, established connection needed - - --function Receive_Bytes (server_stream : Stream_Access; bytes_wanted : in Types.Uint32; dynamic_buffer : in out Byte_Buffer.Buffer) return Types.uint32; + function Connect (client : in out Socket_Type; port : Port_Type; address : in out Sock_Addr_Type) return Stream_Access; --builds the connection and then returns stream access procedure Read_Packet (server_stream : Stream_Access; dynamic_buffer : in out Byte_Buffer.Buffer; local_mailbox : in out Mailbox.Mailbox ); - --procedure read_payload (dynamic_buffer : in out Byte_Buffer.Buffer; payload_length : Types.Uint32; package_ID : Types.Uint8; local_mailbox : in out Mailbox.Mailbox); - end Tcp_Client; diff --git a/external/controller/src/webots_thread/webots_thread.adb b/external/controller/src/webots_thread/webots_thread.adb index 0a3a1da..f37da3d 100644 --- a/external/controller/src/webots_thread/webots_thread.adb +++ b/external/controller/src/webots_thread/webots_thread.adb @@ -4,7 +4,7 @@ package body Webots_Thread is begin - Webots_Channel := Tcp_Client.Connect (Webots_Client, 9999, Webots_Address); + Webots_Channel := Tcp_Client.Connect (Webots_Client, 5555, Webots_Address); while true loop Tcp_Client.Read_Packet (Webots_Channel, Webots_Vector_Buffer, Webots_Mailbox); diff --git a/external/harness/Makefile b/external/harness/Makefile new file mode 100644 index 0000000..f854eb2 --- /dev/null +++ b/external/harness/Makefile @@ -0,0 +1,5 @@ +all: + gprbuild -p -Pharness +clean: + -gprclean -Pharness + rm -rf harness/obj/* diff --git a/external/harness/harness.gpr b/external/harness/harness.gpr new file mode 100644 index 0000000..590bb9a --- /dev/null +++ b/external/harness/harness.gpr @@ -0,0 +1,24 @@ +with "aunit.gpr"; + +project Harness is + + for Source_Dirs use ("src/**", "../controller/src/backend_thread/**", "../controller/src/tcp_client/**", "../controller/src/webots_thread/**"); + for Object_Dir use "obj"; + for Main use ("test_main.adb"); + for Exec_Dir use "."; + + package Linker is + for Default_Switches ("ada") use ("-g"); + end Linker; + + package Binder is + for Default_Switches ("ada") use ("-E", "-static"); + end Binder; + + package Compiler is + for Default_Switches ("ada") use + ("-gnatQ", "-gnatf", "-gnato", "-gnat2012"); + end Compiler; + +end Harness; + diff --git a/external/harness/src/buffer_suite/buffer_suite.adb b/external/harness/src/buffer_suite/buffer_suite.adb new file mode 100644 index 0000000..17708b5 --- /dev/null +++ b/external/harness/src/buffer_suite/buffer_suite.adb @@ -0,0 +1,19 @@ +with buffer_tests; + +package body buffer_suite is + + use AUnit.Test_Suites; + + -- Statically allocate test suite: + Result : aliased Test_Suite; + + -- Statically allocate test cases: + Test_1 : aliased buffer_tests.buffer_test; + + function Suite return Access_Test_Suite is + begin + Add_Test (Result'Access, Test_1'Access); + return Result'Access; + end Suite; + +end buffer_suite; diff --git a/external/harness/src/buffer_suite/buffer_suite.ads b/external/harness/src/buffer_suite/buffer_suite.ads new file mode 100644 index 0000000..d29238c --- /dev/null +++ b/external/harness/src/buffer_suite/buffer_suite.ads @@ -0,0 +1,7 @@ +with AUnit.Test_Suites; + +package buffer_suite is + + function Suite return AUnit.Test_Suites.Access_Test_Suite; + +end buffer_suite; diff --git a/external/harness/src/buffer_suite/buffer_tests.adb b/external/harness/src/buffer_suite/buffer_tests.adb new file mode 100644 index 0000000..4f1a84e --- /dev/null +++ b/external/harness/src/buffer_suite/buffer_tests.adb @@ -0,0 +1,90 @@ +with AUnit.Assertions; use AUnit.Assertions; +with types; use types; +with byte_buffer; use byte_buffer; + +package body buffer_tests is + + procedure Register_Tests (T: in out buffer_test) is + use AUnit.Test_Cases.Registration; + begin + -- Repeat for each test routine: + Register_Routine (T, Test_Read_Write_Uint8'Access, "Test write and read uint8 from buffer."); + Register_Routine (T, Test_Read_Write_Uint16'Access, "Test write and read uint16 from buffer."); + Register_Routine (T, Test_Read_Write_Uint32'Access, "Test write and read uint32 from buffer."); + Register_Routine (T, Test_Read_Write_Uint64'Access, "Test write and read uint64 from buffer."); + Register_Routine (T, Test_Read_Write_Payload'Access, "Test write and read payload from buffer."); + end Register_Tests; + + -- Identifier of test case + function Name (T: buffer_test) return Test_String is + begin + return Format ("Buffer Tests"); + end Name; + + procedure Test_Read_Write_Uint8 (T : in out Test_Cases.Test_Case'Class) is + b : Buffer; + u8, u8t : uint8; + begin + b.Index := 0; + u8 := 230; + b.write_uint8 (u8); + b.read_uint8 (u8t); + + Assert (u8 = u8t, "Writing and reading uint8 does not work"); + end Test_Read_Write_Uint8; + + procedure Test_Read_Write_Uint16 (T : in out Test_Cases.Test_Case'Class) is + b : Buffer; + u16, u16t : uint16; + begin + b.Index := 0; + u16 := 5000; + b.write_uint16 (u16); + b.read_uint16 (u16t); + + Assert (u16 = u16t, "Writing and reading uint16 does not work"); + end Test_Read_Write_Uint16; + + procedure Test_Read_Write_Uint32 (T : in out Test_Cases.Test_Case'Class) is + b : Buffer; + u32, u32t : uint32; + begin + b.Index := 0; + u32 := 200000; + b.write_uint32 (u32); + b.read_uint32 (u32t); + + Assert (u32 = u32t, "Writing and reading uint32 does not work"); + end Test_Read_Write_Uint32; + + procedure Test_Read_Write_Uint64 (T : in out Test_Cases.Test_Case'Class) is + b : Buffer; + u64, u64t : uint64; + begin + b.Index := 0; + u64 := 5000000; + b.write_uint64 (u64); + b.read_uint64 (u64t); + + Assert (u64 = u64t, "Writing and reading uint64 does not work"); + end Test_Read_Write_Uint64; + + procedure Test_Read_Write_Payload (T : in out Test_Cases.Test_Case'Class) is + b : Buffer; + p_in, p_out : access payload := new payload (0 .. 31); + u8 : uint8 := 0; + begin + for I in p_in'Range loop + p_in (I) := u8; + u8 := u8 + 1; + end loop; + + b.write_payload (p_in); + b.read_payload (p_out); + + for I in p_out'Range loop + Assert (p_out (I) = p_in (I), "Writing and reading payload does not work"); + end loop; + end Test_Read_Write_Payload; + +end buffer_tests; diff --git a/external/harness/src/buffer_suite/buffer_tests.ads b/external/harness/src/buffer_suite/buffer_tests.ads new file mode 100644 index 0000000..90f41b5 --- /dev/null +++ b/external/harness/src/buffer_suite/buffer_tests.ads @@ -0,0 +1,22 @@ +with AUnit; use AUnit; +with AUnit.Test_Cases; use AUnit.Test_Cases; +with types; use types; + +package buffer_tests is + + type buffer_test is new Test_Cases.Test_Case with null record; + + procedure Register_Tests (T : in out buffer_test); + -- Register routines to be run + + function Name (T : buffer_test) return Message_String; + -- Provide name identifying the test case + + -- Test Routines: + procedure Test_Read_Write_Uint8 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Read_Write_Uint16 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Read_Write_Uint32 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Read_Write_Uint64 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Read_Write_Payload (T : in out Test_Cases.Test_Case'Class); + +end buffer_tests; diff --git a/external/harness/src/composite_suite.adb b/external/harness/src/composite_suite.adb new file mode 100644 index 0000000..46e9c25 --- /dev/null +++ b/external/harness/src/composite_suite.adb @@ -0,0 +1,22 @@ +with tcp_suite; +with buffer_suite; +with types_suite; +with AUnit.Tests; + + +package body composite_suite is + use Test_Suites; + use tcp_suite; + use buffer_suite; + use types_suite; + + function Suite return Access_Test_Suite is + Result : Access_Test_Suite := AUnit.Test_Suites.New_Suite; + begin + Result.Add_Test (tcp_suite.Suite); + Result.Add_Test (buffer_suite.Suite); + Result.Add_Test (types_suite.Suite); + return Result; + end Suite; + +end composite_suite; diff --git a/external/harness/src/composite_suite.ads b/external/harness/src/composite_suite.ads new file mode 100644 index 0000000..7ebf840 --- /dev/null +++ b/external/harness/src/composite_suite.ads @@ -0,0 +1,6 @@ +with AUnit.Test_Suites; +with AUnit; use AUnit; + +package composite_suite is + function Suite return AUnit.Test_Suites.Access_Test_Suite; +end composite_suite; diff --git a/external/harness/src/tcp_suite/tcp_suite.adb b/external/harness/src/tcp_suite/tcp_suite.adb new file mode 100644 index 0000000..516c26d --- /dev/null +++ b/external/harness/src/tcp_suite/tcp_suite.adb @@ -0,0 +1,19 @@ +with tcp_tests; + +package body tcp_suite is + + use AUnit.Test_Suites; + + -- Statically allocate test suite: + Result : aliased Test_Suite; + + -- Statically allocate test cases: + Test_1 : aliased tcp_tests.tcp_test; + + function Suite return Access_Test_Suite is + begin + Add_Test (Result'Access, Test_1'Access); + return Result'Access; + end Suite; + +end tcp_suite; diff --git a/external/harness/src/tcp_suite/tcp_suite.ads b/external/harness/src/tcp_suite/tcp_suite.ads new file mode 100644 index 0000000..15c706f --- /dev/null +++ b/external/harness/src/tcp_suite/tcp_suite.ads @@ -0,0 +1,7 @@ +with AUnit.Test_Suites; + +package tcp_suite is + + function Suite return AUnit.Test_Suites.Access_Test_Suite; + +end tcp_suite; diff --git a/external/harness/src/tcp_suite/tcp_tests.adb b/external/harness/src/tcp_suite/tcp_tests.adb new file mode 100644 index 0000000..4ffada0 --- /dev/null +++ b/external/harness/src/tcp_suite/tcp_tests.adb @@ -0,0 +1,27 @@ +with AUnit.Assertions; use AUnit.Assertions; + +package body tcp_tests is + + -- Register test routines to call + procedure Register_Tests (T: in out tcp_test) is + use AUnit.Test_Cases.Registration; + begin + -- Repeat for each test routine: + Register_Routine (T, Sample_Test'Access, "This is an empty sample test, because Ada wants at least one test per suite to compile."); + end Register_Tests; + + -- Identifier of test case + function Name (T: tcp_test) return Test_String is + begin + return Format ("TCP Tests"); + end Name; + + procedure Sample_Test (T : in out Test_Cases.Test_Case'Class) is + local : Integer; + begin + local := 1; + + Assert (local = 1, "This should never happen."); + end Sample_Test; + +end tcp_tests; diff --git a/external/harness/src/tcp_suite/tcp_tests.ads b/external/harness/src/tcp_suite/tcp_tests.ads new file mode 100644 index 0000000..76dfb7a --- /dev/null +++ b/external/harness/src/tcp_suite/tcp_tests.ads @@ -0,0 +1,19 @@ +with AUnit; use AUnit; +with AUnit.Test_Cases; use AUnit.Test_Cases; +with tcp_client; use tcp_client; +with types; use types; + +package tcp_tests is + + type tcp_test is new Test_Cases.Test_Case with null record; + + procedure Register_Tests (T : in out tcp_test); + -- Register routines to be run + + function Name (T : tcp_test) return Message_String; + -- Provide name identifying the test case + + -- Test Routines: + procedure Sample_Test (T : in out Test_Cases.Test_Case'Class); + +end tcp_tests; diff --git a/external/harness/src/test_main.adb b/external/harness/src/test_main.adb new file mode 100644 index 0000000..dbda8ae --- /dev/null +++ b/external/harness/src/test_main.adb @@ -0,0 +1,12 @@ +with AUnit.Reporter.Text; +with AUnit.Run; +with composite_suite; use composite_suite; +with Ada.Task_Identification; use Ada.Task_Identification; + +procedure Test_Main is + procedure Runner is new AUnit.Run.Test_Runner (composite_suite.Suite); + Reporter : AUnit.Reporter.Text.Text_Reporter; +begin + Runner (Reporter); + return; +end Test_Main; diff --git a/external/harness/src/types_suite/types_suite.adb b/external/harness/src/types_suite/types_suite.adb new file mode 100644 index 0000000..caeb47e --- /dev/null +++ b/external/harness/src/types_suite/types_suite.adb @@ -0,0 +1,19 @@ +with types_tests; + +package body types_suite is + + use AUnit.Test_Suites; + + -- Statically allocate test suite: + Result : aliased Test_Suite; + + -- Statically allocate test cases: + Test_1 : aliased types_tests.types_test; + + function Suite return Access_Test_Suite is + begin + Add_Test (Result'Access, Test_1'Access); + return Result'Access; + end Suite; + +end types_suite; diff --git a/external/harness/src/types_suite/types_suite.ads b/external/harness/src/types_suite/types_suite.ads new file mode 100644 index 0000000..c5250d4 --- /dev/null +++ b/external/harness/src/types_suite/types_suite.ads @@ -0,0 +1,7 @@ +with AUnit.Test_Suites; + +package types_suite is + + function Suite return AUnit.Test_Suites.Access_Test_Suite; + +end types_suite; diff --git a/external/harness/src/types_suite/types_tests.adb b/external/harness/src/types_suite/types_tests.adb new file mode 100644 index 0000000..2a76892 --- /dev/null +++ b/external/harness/src/types_suite/types_tests.adb @@ -0,0 +1,168 @@ +with AUnit.Assertions; use AUnit.Assertions; +with types; use types; + +package body types_tests is + + -- Register test routines to call + procedure Register_Tests (T: in out types_test) is + use AUnit.Test_Cases.Registration; + begin + -- Repeat for each test routine: + Register_Routine (T, Test_Uint16_To_Octet2'Access, "Test uint16 to Octet_2 and back."); + Register_Routine (T, Test_Uint32_To_Octet4'Access, "Test uint32 to Octet_4 and back."); + Register_Routine (T, Test_Uint64_To_Octet8'Access, "Test uint64 to Octet_8 and back."); + + Register_Routine (T, Test_Octet2_To_Uint16'Access, "Test Octet_2 to uint16 and back."); + Register_Routine (T, Test_Octet4_To_Uint32'Access, "Test Octet_4 to uint32 and back."); + Register_Routine (T, Test_Octet8_To_Uint64'Access, "Test Octet_8 to uint64 and back."); + + Register_Routine (T, Test_Uint64_To_Float64'Access, "Test uint64 to float64 and back."); + Register_Routine (T, Test_Float64_To_Uint64'Access, "Test float64 to uint64 and back."); + + Register_Routine (T, Test_Byte_Order_16'Access, "Test hton16 and ntoh16."); + Register_Routine (T, Test_Byte_Order_32'Access, "Test hton32 and ntoh32."); + Register_Routine (T, Test_Byte_Order_64'Access, "Test hton64 and ntoh64."); + end Register_Tests; + + -- Identifier of test case + function Name (T: types_test) return Test_String is + begin + return Format ("Types Tests"); + end Name; + + procedure Test_Uint16_To_Octet2 (T : in out Test_Cases.Test_Case'Class) is + u16, u16t : uint16; + o2 : Octets_2; + begin + u16 := 5000; + o2 := uint16_to_octets (u16); + u16t := octets_to_uint16 (o2); + + Assert (u16 = u16t, "Converting uint16 to octets and back does not work"); + end Test_Uint16_To_Octet2; + + procedure Test_Uint32_To_Octet4 (T : in out Test_Cases.Test_Case'Class) is + u32, u32t : uint32; + o4 : Octets_4; + begin + u32 := 100000; + o4 := uint32_to_octets (u32); + u32t := octets_to_uint32 (o4); + + Assert (u32 = u32t, "Converting uint32 to octets and back does not work"); + end Test_Uint32_To_Octet4; + + procedure Test_Uint64_To_Octet8 (T : in out Test_Cases.Test_Case'Class) is + u64, u64t : uint64; + o8 : Octets_8; + begin + u64 := 5000000; + o8 := uint64_to_octets (u64); + u64t := octets_to_uint64 (o8); + + Assert (u64 = u64t, "Converting uint64 to octets and back does not work"); + end Test_Uint64_To_Octet8; + + procedure Test_Octet2_To_Uint16 (T : in out Test_Cases.Test_Case'Class) is + o2, o2t : Octets_2; + u16 : uint16; + begin + o2 (0) := 150; + o2 (1) := 35; + u16 := octets_to_uint16 (o2); + o2t := uint16_to_octets (u16); + + Assert (o2 (0) = o2t (0) and o2 (1) = o2t (1), + "Converting Octets_2 to uint16 and back does not work"); + end Test_Octet2_To_Uint16; + + procedure Test_Octet4_To_Uint32 (T : in out Test_Cases.Test_Case'Class) is + o4, o4t : Octets_4; + u32 : uint32; + begin + o4 (0) := 150; + o4 (1) := 35; + o4 (2) := 255; + o4 (3) := 1; + u32 := octets_to_uint32 (o4); + o4t := uint32_to_octets (u32); + + Assert (o4 (0) = o4t (0) and o4 (1) = o4t (1) and + o4 (2) = o4t (2) and o4 (3) = o4t (3), + "Converting Octets_4 to uint32 and back does not work"); + end Test_Octet4_To_Uint32; + + procedure Test_Octet8_To_Uint64 (T : in out Test_Cases.Test_Case'Class) is + o8, o8t : Octets_8; + u64 : uint64; + begin + o8 (0) := 150; + o8 (1) := 35; + o8 (2) := 255; + o8 (3) := 1; + o8 (4) := 90; + o8 (5) := 200; + o8 (6) := 0; + o8 (7) := 70; + u64 := octets_to_uint64 (o8); + o8t := uint64_to_octets (u64); + + Assert (o8 (0) = o8t (0) and o8 (1) = o8t (1) and + o8 (2) = o8t (2) and o8 (3) = o8t (3) and + o8 (4) = o8t (4) and o8 (5) = o8t (5) and + o8 (6) = o8t (6) and o8 (7) = o8t (7), + "Converting Octets_4 to uint64 and back does not work"); + end Test_Octet8_To_Uint64; + + procedure Test_Uint64_To_Float64 (T : in out Test_Cases.Test_Case'Class) is + u64, u64t : uint64; + f64 : float64; + begin + u64 := 1234567; + f64 := uint64_to_float64 (u64); + u64t := float64_to_uint64 (f64); + Assert (u64 = u64t, "Converting uint64 to float64 and back doe not work"); + end Test_Uint64_To_Float64; + + procedure Test_Float64_To_Uint64 (T : in out Test_Cases.Test_Case'Class) is + f64, f64t : float64; + u64 : uint64; + begin + f64 := 3.004972; + u64 := float64_to_uint64 (f64); + f64t := uint64_to_float64 (u64); + + Assert (f64 = f64t, "Converting float64 to uint64 and back doe not work"); + end Test_Float64_To_Uint64; + + procedure Test_Byte_Order_16 (T : in out Test_Cases.Test_Case'Class) is + u16i, u16m, u16o : uint16; + begin + u16i := 45298; + u16m := hton16 (u16i); + u16o := ntoh16 (u16m); + + Assert (u16i = u16o, "hton16 and ntoh16 do not work."); + end Test_Byte_Order_16; + + procedure Test_Byte_Order_32 (T : in out Test_Cases.Test_Case'Class) is + u32i, u32m, u32o : uint32; + begin + u32i := 3568298; + u32m := hton32 (u32i); + u32o := ntoh32 (u32m); + + Assert (u32i = u32o, "hton32 and ntoh32 do not work."); + end Test_Byte_Order_32; + + procedure Test_Byte_Order_64 (T : in out Test_Cases.Test_Case'Class) is + u64i, u64m, u64o : uint64; + begin + u64i := 6568298; + u64m := hton64 (u64i); + u64o := ntoh64 (u64m); + + Assert (u64i = u64o, "hton64 and ntoh64 do not work."); + end Test_Byte_Order_64; + +end types_tests; diff --git a/external/harness/src/types_suite/types_tests.ads b/external/harness/src/types_suite/types_tests.ads new file mode 100644 index 0000000..1e406ea --- /dev/null +++ b/external/harness/src/types_suite/types_tests.ads @@ -0,0 +1,31 @@ +with AUnit; use AUnit; +with AUnit.Test_Cases; use AUnit.Test_Cases; +with types; use types; + +package types_tests is + + type types_test is new Test_Cases.Test_Case with null record; + + procedure Register_Tests (T : in out types_test); + -- Register routines to be run + + function Name (T : types_test) return Message_String; + -- Provide name identifying the test case + + -- Test Routines: + procedure Test_Uint16_To_Octet2 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Uint32_To_Octet4 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Uint64_To_Octet8 (T : in out Test_Cases.Test_Case'Class); + + procedure Test_Octet2_To_Uint16 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Octet4_To_Uint32 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Octet8_To_Uint64 (T : in out Test_Cases.Test_Case'Class); + + procedure Test_Uint64_To_Float64 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Float64_To_Uint64 (T : in out Test_Cases.Test_Case'Class); + + procedure Test_Byte_Order_16 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Byte_Order_32 (T : in out Test_Cases.Test_Case'Class); + procedure Test_Byte_Order_64 (T : in out Test_Cases.Test_Case'Class); + +end types_tests; diff --git a/shared/Makefile b/shared/Makefile new file mode 100644 index 0000000..de31d8a --- /dev/null +++ b/shared/Makefile @@ -0,0 +1,72 @@ +# Copyright 1996-2020 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +### Generic Makefile.include for Webots controllers, physics plugins, robot +### window libraries, remote control libraries and other libraries +### to be used with GNU make +### +### Platforms: Windows, macOS, Linux +### Languages: C, C++ +### +### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer +### Edmund Ronald, Sergei Poskriakov +### +###----------------------------------------------------------------------------- +### +### This file is meant to be included from the Makefile files located in the +### Webots projects subdirectories. It is possible to set a number of variables +### to customize the build process, i.e., add source files, compilation flags, +### include paths, libraries, etc. These variables should be set in your local +### Makefile just before including this Makefile.include. This Makefile.include +### should never be modified. +### +### Here is a description of the variables you may set in your local Makefile: +### +### ---- C Sources ---- +### if your program uses several C source files: +### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c +### +### ---- C++ Sources ---- +### if your program uses several C++ source files: +### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ +### +### ---- Compilation options ---- +### if special compilation flags are necessary: +### CFLAGS = -Wno-unused-result +### +### ---- Linked libraries ---- +### if your program needs additional libraries: +### INCLUDE = -I"/my_library_path/include" +### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library +### +### ---- Linking options ---- +### if special linking flags are needed: +### LFLAGS = -s +### +### ---- Webots included libraries ---- +### if you want to use the Webots C API in your C++ controller program: +### USE_C_API = true +### +### ---- Debug mode ---- +### if you want to display the gcc command line for compilation and link, as +### well as the rm command details used for cleaning: +### VERBOSE = 1 +### +###----------------------------------------------------------------------------- + +### Do not modify: this includes Webots global Makefile.include +space := +space += +WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/shared/buffer_reader.cpp b/shared/buffer_reader.cpp index 6fcd8b1..ee842e6 100644 --- a/shared/buffer_reader.cpp +++ b/shared/buffer_reader.cpp @@ -73,6 +73,7 @@ buffer_reader& buffer_reader::operator>>(double& data) return *this; } +/* buffer_reader& buffer_reader::operator>>(std::string& string) { std::size_t size = 0; @@ -81,8 +82,8 @@ buffer_reader& buffer_reader::operator>>(std::string& string) for(std::size_t i = 0; i < size; ++i) { - //*this >> string[i]; + *this >> string[i]; } return *this; -} +}*/ diff --git a/shared/buffer_writer.cpp b/shared/buffer_writer.cpp index 031a210..4204b4e 100644 --- a/shared/buffer_writer.cpp +++ b/shared/buffer_writer.cpp @@ -57,13 +57,14 @@ buffer_writer& buffer_writer::operator<<(double data) return *this; } +/* buffer_writer& buffer_writer::operator<<(const std::string& string) { *this << string.size(); for(auto element:string) { - //*this << element; + *this << element; } return *this; -} +}*/ diff --git a/test/main.cpp b/test/main.cpp new file mode 100644 index 0000000..b32cdbc --- /dev/null +++ b/test/main.cpp @@ -0,0 +1,279 @@ +#define BOOST_TEST_MODULE Test +#include + +#include + +#include "../shared/buffer_reader.hpp" +#include "../shared/buffer_writer.hpp" +#include "../shared/message.hpp" + +BOOST_AUTO_TEST_CASE(buffer_write_read_integer) +{ + const uint8_t wuint8 = 0xFF - 1; + const uint16_t wuint16 = 0xFFFF - 1; + const uint32_t wuint32 = 0xFFFFFFFF - 1; + const uint64_t wuint64 = 0xFFFFFFFFFFFFFFFF - 1; + const double wdouble = 3.1415; + + uint8_t ruint8 = 0; + uint16_t ruint16 = 0; + uint32_t ruint32 = 0; + uint64_t ruint64 = 0; + double rdouble = 0; + auto total = sizeof(uint8_t) + sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint64_t) + sizeof(double); + + buffer buffer; + + buffer_writer writer(buffer); + writer << wuint8 << wuint16 << wuint32 << wuint64 << wdouble; + + buffer_reader reader(buffer); + reader >> ruint8 >> ruint16 >> ruint32 >> ruint64 >> rdouble; + + BOOST_TEST(ruint8 != 0); + BOOST_TEST(ruint16 != 0); + BOOST_TEST(ruint32 != 0); + BOOST_TEST(ruint64 != 0); + BOOST_TEST(rdouble != 0); + + BOOST_TEST(wuint8 == ruint8); + BOOST_TEST(wuint16 == ruint16); + BOOST_TEST(wuint32 == ruint32); + BOOST_TEST(wuint64 == ruint64); + BOOST_TEST(wdouble == rdouble); + + BOOST_TEST(writer.written() == total); + BOOST_TEST(reader.read() == total); +} + +BOOST_AUTO_TEST_CASE(buffer_write_read_string) +{ + buffer buffer; + + std::string wstring = "Hello World"; + buffer_writer writer(buffer); + writer << wstring; + + std::string rstring; + buffer_reader reader(buffer); + reader >> rstring; + + //BOOST_TEST(wstring == rstring); +} + +BOOST_AUTO_TEST_CASE(buffer_write_read_array) +{ + buffer buffer; + + std::array warray = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + buffer_writer writer(buffer); + writer << warray; + + std::array rarray; + buffer_reader reader(buffer); + reader >> rarray; + + BOOST_TEST(warray == rarray); +} + +BOOST_AUTO_TEST_CASE(buffer_write_read_vector) +{ + buffer buffer; + + std::vector wvector = {0, 2, 4, 6, 8}; + buffer_writer writer(buffer); + writer << wvector; + + std::vector rvector; + buffer_reader reader(buffer); + reader >> rvector; + + BOOST_TEST(wvector == rvector); +} + +BOOST_AUTO_TEST_CASE(message_header_write_read) +{ + const message_size size = 0xFF55; + const message_id id = message_id::PING; + buffer buffer; + + message_header write_header; + write_header.size = size; + write_header.id = id; + + buffer_writer writer(buffer); + writer << write_header; + + buffer_reader reader(buffer); + message_header read_header; + reader >> read_header; + + BOOST_TEST(read_header.id == id); + BOOST_TEST(read_header.size == size); +} + +BOOST_AUTO_TEST_CASE(empty_messages_empty) +{ + ping_message ping; + nop_message nop; + + BOOST_TEST(nop.body_size() == 0); + BOOST_TEST(ping.body_size() == 0); +} + +BOOST_AUTO_TEST_CASE(message_unmatching_ids) +{ + buffer buffer; + + buffer_writer writer(buffer); + writer << ping_message(); + + ping_message ping; + nop_message nop; + buffer_reader reader(buffer); + + BOOST_CHECK_THROW(reader >> nop, std::runtime_error); + BOOST_CHECK_NO_THROW(reader >> ping); +} + +BOOST_AUTO_TEST_CASE(webots_velocity_message_write_read) +{ + webots_velocity_message write_msg; + write_msg.right_speed = 2.5; + write_msg.left_speed = 4.1; + + buffer buffer; + buffer_writer writer(buffer); + writer << write_msg; + + webots_velocity_message read_msg; + buffer_reader reader(buffer); + reader >> read_msg; + + BOOST_TEST(read_msg.right_speed == write_msg.right_speed); + BOOST_TEST(read_msg.left_speed == write_msg.left_speed); +} + +BOOST_AUTO_TEST_CASE(external_distance_sensor_message_write_read) +{ + external_distance_sensor_message write_msg; + for(int i = 0; i<9; i++){ + write_msg.data[i] = static_cast(i) + 0.3; + } + + buffer buffer; + buffer_writer writer(buffer); + writer << write_msg; + + external_distance_sensor_message read_msg; + buffer_reader reader(buffer); + reader >> read_msg; + + for(int i = 0; i<9; i++){ + BOOST_TEST(read_msg.data[i] == write_msg.data[i]); + } +} + +BOOST_AUTO_TEST_CASE(external_light_sensor_message_write_read) +{ + external_light_sensor_message write_msg; + for(int i = 0; i<1; i++){ + write_msg.data[i] = static_cast(i) + 0.6; + } + + buffer buffer; + buffer_writer writer(buffer); + writer << write_msg; + + external_light_sensor_message read_msg; + buffer_reader reader(buffer); + reader >> read_msg; + + for(int i = 0; i<1; i++){ + BOOST_TEST(read_msg.data[i] == write_msg.data[i]); + } +} + +BOOST_AUTO_TEST_CASE(external_image_data_message_write_read) +{ + external_image_data_message write_msg; + unsigned char test[26]; + for(int i = 0; i<26; i++){ + test[i] = 65 + i; + } + std::vector vec(test, test + 26); + write_msg.pixel = vec; + + buffer buffer; + buffer_writer writer(buffer); + writer << write_msg; + + external_image_data_message read_msg; + buffer_reader reader(buffer); + reader >> read_msg; + + for(int i = 0; i<26; i++){ + BOOST_TEST(read_msg.pixel[i] == write_msg.pixel[i]); + } +} + +BOOST_AUTO_TEST_CASE(message_header_size) +{ + BOOST_TEST(sizeof(message_header) == 5); +} + +BOOST_AUTO_TEST_CASE(empty_message_size) +{ + BOOST_TEST((empty_message{}).body_size() == 0); +} + +BOOST_AUTO_TEST_CASE(test_message_sizes) +{ + #pragma pack(push, 1) + struct test_message final : public empty_message + { + }; + #pragma pack(pop) + + const auto msg = test_message{}; + + BOOST_TEST(msg.body_size() == 0); + BOOST_TEST(msg.size() == sizeof(message_header)); +} + +BOOST_AUTO_TEST_CASE(test_message2_sizes) +{ + #pragma pack(push, 1) + struct test_message2 final : public basic_message + { + std::uint8_t a; + std::uint32_t b; + std::uint8_t c; + float d; + std::array e; + double f; + std::uint8_t g; + std::array h; + }; + #pragma pack(pop) + + const auto size = sizeof(std::uint8_t) + + sizeof(std::uint32_t) + + sizeof(std::uint8_t) + + sizeof(float) + + sizeof(std::array) + + sizeof(double) + + sizeof(std::uint8_t) + + sizeof(std::array); + + const auto msg = test_message2{}; + + BOOST_TEST(msg.body_size() == size); + BOOST_TEST(msg.size() == sizeof(message_header) + size); +} + +BOOST_AUTO_TEST_CASE(webots_velocity_message_size) +{ + webots_velocity_message msg; + BOOST_TEST(msg.body_size() == 2 * sizeof(double)); +} diff --git a/webots/.cabservice.wbproj b/webots/.cabservice.wbproj new file mode 100644 index 0000000..beaa309 --- /dev/null +++ b/webots/.cabservice.wbproj @@ -0,0 +1,13 @@ +Webots Project File version R2020a +perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000012400000362fc0200000001fb0000001400540065007800740045006400690074006f0072010000001c00000362000000a200ffffff00000003000007380000005efc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007380000005400ffffff0000060e0000036200000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000118000002d60100000006010000000101 +sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201 +minimizedPerspectives: 000000ff00000000fd00000003000000000000020000000448fc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000024700000341fc0200000001fb0000001400540065007800740045006400690074006f0072010000001600000341000000a200ffffff000000030000073800000085fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007380000005400ffffff000004eb0000034100000004000000040000000100000008fc00000000 +maximizedDockId: -1 +centralWidgetVisible: 1 +selectionDisabled: 0 +viewpointLocked: 0 +orthographicViewHeight: 1 +textFiles: 0 "webots/controllers/controller/controller.cpp" +renderingDevicePerspectives: Cab1:camera;1;3.2;0;0 +renderingDevicePerspectives: Cab2:camera;1;3.2;0;0.127016 diff --git a/webots/cabservice.wbt b/webots/cabservice.wbt new file mode 100644 index 0000000..eb43698 --- /dev/null +++ b/webots/cabservice.wbt @@ -0,0 +1,2258 @@ +#VRML_SIM R2020a utf8 +WorldInfo { + basicTimeStep 16 +} +Viewpoint { + orientation 0.03808682417150016 0.7714041274115354 0.635204743399305 3.119688131672871 + position 6.409620980680648 139.501828724283 -82.55227138078858 +} +TexturedBackground { + luminosity 2 + reflections FALSE +} +DEF Cab1 Robot { + translation 0 0.4 -29.2 + rotation 0 1 0 -1.5723853071795864 + children [ + DEF cam_f Camera { + translation 0 0.1 1 + rotation 0 1 0 3.14 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.4 0 0 + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + density 1 + } + fieldOfView 0.75 + width 18 + height 10 + far 10 + } + DEF ls_f LightSensor { + translation 0 0 1 + rotation 0 1 0 -1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ls_front" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fl DistanceSensor { + translation 0.45 0.2 1 + rotation 0 1 0 -1.047 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fl" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_lf DistanceSensor { + translation 0.5 0.2 0.95 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_lf" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_lc DistanceSensor { + translation 0.5 0.2 0 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_lc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fr DistanceSensor { + translation -0.45 0.2 1 + rotation 0 1 0 -2.09 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fr" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_rf DistanceSensor { + translation -0.5 0.2 0.95 + rotation 0 1 0 -3.14 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_rf" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_rc DistanceSensor { + translation -0.5 0.2 0 + rotation 0 1 0 -3.14 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_rc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fc DistanceSensor { + translation 0 0.2 1 + rotation 0 1 0 -1.57 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fcr DistanceSensor { + translation -0.225 0.2 1 + rotation 0 1 0 -1.833 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fcr" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fcl DistanceSensor { + translation 0.225 0.2 1 + rotation 0 1 0 -1.309 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fcl" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + Transform { + translation 0 0.5 -0.3 + rotation 1 0 0 0 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + transparency 0.3 + } + geometry Box { + size 0.9 0.5 1.2 + } + } + ] + } + Transform { + translation 0 0.8 -0.3 + rotation 1 0 0 0 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 0 0 + } + geometry Box { + size 1 0.1 1.3 + } + } + ] + } + DEF wheelFL HingeJoint { + jointParameters HingeJointParameters { + anchor 0.55 -0.1 0.55 + } + device [ + DEF wheelFL RotationalMotor { + name "wheelFL" + maxTorque 30 + } + ] + endPoint DEF wheelFL Solid { + translation 0.55 -0.1 0.55 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelFL" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelRL HingeJoint { + jointParameters HingeJointParameters { + anchor 0.55 -0.1 -0.7 + } + device [ + DEF wheelRL RotationalMotor { + name "wheelRL" + maxTorque 30 + } + ] + endPoint DEF wheelRL Solid { + translation 0.55 -0.1 -0.7 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelRL" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelRR HingeJoint { + jointParameters HingeJointParameters { + anchor -0.55 -0.1 -0.7 + } + device [ + DEF wheelRR RotationalMotor { + name "wheelRR" + maxTorque 30 + } + ] + endPoint DEF wheelRR Solid { + translation -0.55 -0.1 -0.7 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelRR" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelFR HingeJoint { + jointParameters HingeJointParameters { + anchor -0.55 -0.1 0.55 + } + device [ + DEF wheelFR RotationalMotor { + name "wheelFR" + maxTorque 30 + } + ] + endPoint DEF wheelFR Solid { + translation -0.55 -0.1 0.55 + rotation 0 0 1 1.5699999999999956 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelFR" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF car_body Shape { + appearance PBRAppearance { + baseColor 1 1 0 + } + geometry Box { + size 1 0.6 2 + } + } + ] + name "Cab1" + boundingObject USE car_body + physics Physics { + density 10 + } + recognitionColors [ + 0 0 0 + ] + controller "controller" +} +DEF Cab2 Robot { + translation 10 0.4 -29.1 + rotation 0 1 0 -1.5723853071795864 + children [ + DEF cam_f Camera { + translation 0 0.1 1 + rotation 0 1 0 3.14 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.4 0 0 + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + ] + boundingObject Cylinder { + height 0.1 + radius 0.05 + } + physics Physics { + density 1 + } + fieldOfView 0.75 + width 18 + height 10 + far 10 + } + DEF ls_f LightSensor { + translation 0 0 1 + rotation 0 1 0 -1.57 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ls_front" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Cylinder { + height 0.1 + radius 0.05 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fl DistanceSensor { + translation 0.45 0.2 1 + rotation 0 1 0 -1.047 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fl" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_lf DistanceSensor { + translation 0.5 0.2 0.95 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_lf" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_lc DistanceSensor { + translation 0.5 0.2 0 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_lc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fr DistanceSensor { + translation -0.45 0.2 1 + rotation 0 1 0 -2.09 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fr" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_rf DistanceSensor { + translation -0.5 0.2 0.95 + rotation 0 1 0 -3.14 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_rf" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_rc DistanceSensor { + translation -0.5 0.2 0 + rotation 0 1 0 -3.14 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_rc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fc DistanceSensor { + translation 0 0.2 1 + rotation 0 1 0 -1.57 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fc" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fcr DistanceSensor { + translation -0.225 0.2 1 + rotation 0 1 0 -1.833 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fcr" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + DEF ds_fcl DistanceSensor { + translation 0.225 0.2 1 + rotation 0 1 0 -1.309 + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + name "ds_fcl" + boundingObject Shape { + appearance PBRAppearance { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + physics Physics { + density 1 + } + lookupTable [ + 0 0 0 + 5 1000 0 + ] + } + Transform { + translation 0 0.5 -0.3 + rotation 1 0 0 0 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + transparency 0.3 + } + geometry Box { + size 0.9 0.5 1.2 + } + } + ] + } + Transform { + translation 0 0.8 -0.3 + rotation 1 0 0 0 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 0 0 + } + geometry Box { + size 1 0.1 1.3 + } + } + ] + } + DEF wheelFL HingeJoint { + jointParameters HingeJointParameters { + anchor 0.55 -0.1 0.55 + } + device [ + DEF wheelFL RotationalMotor { + name "wheelFL" + maxTorque 30 + } + ] + endPoint DEF wheelFL Solid { + translation 0.55 -0.1 0.55 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelFL" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelRL HingeJoint { + jointParameters HingeJointParameters { + anchor 0.55 -0.1 -0.7 + } + device [ + DEF wheelRL RotationalMotor { + name "wheelRL" + maxTorque 30 + } + ] + endPoint DEF wheelRL Solid { + translation 0.55 -0.1 -0.7 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelRL" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelRR HingeJoint { + jointParameters HingeJointParameters { + anchor -0.55 -0.1 -0.7 + } + device [ + DEF wheelRR RotationalMotor { + name "wheelRR" + maxTorque 30 + } + ] + endPoint DEF wheelRR Solid { + translation -0.55 -0.1 -0.7 + rotation 0 0 1 1.5699999999999963 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelRR" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF wheelFR HingeJoint { + jointParameters HingeJointParameters { + anchor -0.55 -0.1 0.55 + } + device [ + DEF wheelFR RotationalMotor { + name "wheelFR" + maxTorque 30 + } + ] + endPoint DEF wheelFR Solid { + translation -0.55 -0.1 0.55 + rotation 0 0 1 1.5699999999999956 + children [ + DEF wheel Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + } + geometry Cylinder { + height 0.1 + radius 0.3 + } + } + ] + name "wheelFR" + boundingObject USE wheel + physics DEF physics_wheel Physics { + density 10 + } + } + } + DEF car_body Shape { + appearance PBRAppearance { + baseColor 1 1 0 + } + geometry Box { + size 1 0.6 2 + } + } + ] + name "Cab2" + boundingObject USE car_body + physics Physics { + density 10 + } + recognitionColors [ + 0 0 0 + ] + controller "controller" +} +DEF environment Solid { + scale 1.09819 1.09819 1.09819 + children [ + DEF PickUp1 BusStop { + translation 0 0 -45 + rotation 0 1 0 3.14159 + name "PickUp1" + bench FALSE + appearance PBRAppearance { + baseColor 0 0 0 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + } + DEF PickUp2 BusStop { + translation 45 0 0 + rotation 0 -1 0 -1.5707953071795862 + name "PickUp2" + bench FALSE + appearance PBRAppearance { + baseColor 1 0 0 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp3 BusStop { + translation 0 0 45 + rotation 0 1 0 -5.307179586466759e-06 + name "PickUp3" + bench FALSE + appearance PBRAppearance { + baseColor 0 1 0 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp4 BusStop { + translation -45 0 0 + rotation 0 1 0 -1.5707953071795862 + name "PickUp4" + bench FALSE + appearance PBRAppearance { + baseColor 0 0 1 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp5 BusStop { + translation 0 0 -71 + rotation 0 1 0 3.14159 + name "PickUp5" + bench FALSE + appearance PBRAppearance { + baseColor 1 1 0 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp6 BusStop { + translation 71 0 0 + rotation 0 -1 0 -1.5707953071795862 + name "PickUp6" + bench FALSE + appearance PBRAppearance { + baseColor 1 0 1 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp7 BusStop { + translation 0 0 71 + rotation 0 1 0 -5.307179586466759e-06 + name "PickUp7" + bench FALSE + appearance PBRAppearance { + baseColor 0 1 1 + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + DEF PickUp8 BusStop { + translation -71 0 0 + rotation 0 1 0 -1.5707953071795862 + name "PickUp8" + bench FALSE + appearance PBRAppearance { + roughness 0.5 + metalness 0 + } + internalAdvertisingTexture [] + externalAdvertisingTexture [] + advertisingLightOn FALSE + } + CurvedRoadSegment { + translation -56 0 33 + rotation 0 1 0 -1.570796 + name "road(1)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation -56 0 -33 + rotation 0 1 0 3.14 + name "road(7)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation -33 0 -56 + rotation 0 1 0 3.14 + name "road(8)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation 33 0 -56 + rotation 0 1 0 1.57 + name "road(9)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation 56 0 -33 + rotation 0 1 0 1.57 + name "road(10)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation 56 0 33 + name "road(11)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation 33 0 56 + name "road(12)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation -33 0 56 + rotation 0 1 0 -1.57 + name "road(13)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + curvatureRadius 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + RoadIntersection { + translation -53 0 53 + rotation 0 1 0 0.785 + roadsWidth 6 + startRoadsLength [ + -0.1 + ] + startRoadsStartLine [ + "" + "" + ] + startRoadsLine [ + RoadLine { + type "continuous" + width 0.3 + } + ] + startRoadBorderWidth 0.5 + bottom TRUE + appearance Asphalt { + colorOverride 0.5 0.5 0 + } + } + RoadIntersection { + translation 53 0 53 + rotation 0 1 0 0.785 + name "road intersection(1)" + roadsWidth 6 + startRoadsLength [ + -0.1 + ] + startRoadsStartLine [ + "" + "" + ] + startRoadsLine [ + RoadLine { + type "continuous" + width 0.3 + } + ] + startRoadBorderWidth 0.5 + bottom TRUE + appearance Asphalt { + colorOverride 0.5 0.5 0 + } + } + RoadIntersection { + translation 53 0 -53 + rotation 0 1 0 0.785 + name "road intersection(2)" + roadsWidth 6 + startRoadsLength [ + -0.1 + ] + startRoadsStartLine [ + "" + "" + ] + startRoadsLine [ + RoadLine { + type "continuous" + width 0.3 + } + ] + startRoadBorderWidth 0.5 + bottom TRUE + appearance Asphalt { + colorOverride 0.5 0.5 0 + } + } + RoadIntersection { + translation -53 0 -53 + rotation 0 1 0 0.785 + name "road intersection(3)" + roadsWidth 6 + startRoadsLength [ + -0.1 + ] + startRoadsStartLine [ + "" + "" + ] + startRoadsLine [ + RoadLine { + type "continuous" + width 0.3 + } + ] + startRoadBorderWidth 0.5 + bottom TRUE + appearance Asphalt { + colorOverride 0.5 0.5 0 + } + } + StraightRoadSegment { + translation -53 0 20 + name "road(20)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation -53 0 20 + rotation 0 1 0 3.14 + width 6 + length 10 + numberOfNewLanes 1 + newLaneLeft FALSE + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation -51.48 0 15.5 + rotation 0 1 0 -0.2617996938995747 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(1)" + } + StraightRoadSegment { + translation -51.5 0 -10 + name "road(17)" + width 9 + numberOfLanes 3 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation -53 0 -20 + name "road(3)" + width 6 + length 10 + numberOfNewLanes 1 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation -51.48 0 -15.5 + rotation 0 1 0 0.2618 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(2)" + } + StraightRoadSegment { + translation -53 0 -50 + name "road(5)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation -50 0 -53 + rotation 0 1 0 1.57 + name "road(4)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation -10 0 -51.5 + rotation 0 1 0 -1.57 + name "road(2)" + width 9 + length 10 + numberOfLanes 3 + numberOfNewLanes 1 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width -1.73472e-18 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation 15.5 0 -51.48 + rotation 0 1 0 1.8326 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(3)" + } + StraightRoadSegment { + translation -10 0 -51.5 + rotation 0 1 0 1.571 + name "road(18)" + width 9 + numberOfLanes 3 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation 10 0 -51.5 + rotation 0 1 0 1.57 + name "road(19)" + width 9 + length 10 + numberOfLanes 3 + numberOfNewLanes 1 + newLaneLeft FALSE + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width -1.73472e-18 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation -15.5 0 -51.48 + rotation 0 1 0 -1.8326 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(4)" + } + CurvedRoadSegment { + translation 20 0 -36.8 + rotation 0 1 0 1.571 + name "road(33)" + width 5.8 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + roadBorderWidth [ + 0 + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + totalAngle 3.14 + subdivision 21 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation -20 0 -26.8 + rotation 0 1 0 1.571 + name "road(35)" + width 5.8 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 40 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + CurvedRoadSegment { + translation -20 0 -36.8 + rotation 0 1 0 -1.571 + name "road(34)" + width 5.8 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + roadBorderWidth [ + 0 + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + totalAngle 3.14 + subdivision 21 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation 20 0 -53 + rotation 0 1 0 1.57 + name "road(21)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation 53 0 -50 + name "road(22)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation 53 0 -20 + name "road(23)" + width 6 + length 10 + numberOfNewLanes 1 + newLaneLeft FALSE + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation 51.48 0 -15.5 + rotation 0 1 0 -0.2617996938995747 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(5)" + } + StraightRoadSegment { + translation 51.5 0 -10 + name "road(24)" + width 9 + numberOfLanes 3 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation 53 0 20 + rotation 0 1 0 3.14 + name "road(25)" + width 6 + length 10 + numberOfNewLanes 1 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation 51.48 0 15.5 + rotation 0 1 0 0.2618 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(6)" + } + StraightRoadSegment { + translation 53 0 20 + name "road(26)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation 20 0 53 + rotation 0 1 0 1.57 + name "road(27)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation 20 0 53 + rotation 0 1 0 -1.57 + name "road(28)" + width 6 + length 10 + numberOfNewLanes 1 + newLaneLeft FALSE + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation 15.5 0 51.48 + rotation 0 1 0 -1.8326 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(7)" + } + StraightRoadSegment { + translation -10 0 51.5 + rotation 0 1 0 1.57 + name "road(29)" + width 9 + numberOfLanes 3 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 20 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + AddLanesRoadSegment { + translation -20 0 53 + rotation 0 1 0 1.57 + name "road(30)" + width 6 + length 10 + numberOfNewLanes 1 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation -15.5 0 51.48 + rotation 0 1 0 1.8326 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 11.6 + } + } + ] + name "solid(8)" + } + StraightRoadSegment { + translation -50 0 53 + rotation 0 1 0 1.57 + name "road(32)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation 53 0 20 + name "road(31)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 30 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation -76 0 -33 + name "road(6)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 66 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation -33 0 -76 + rotation 0 1 0 1.57 + name "road(14)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 66 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation 76 0 -33 + name "road(15)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 66 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + StraightRoadSegment { + translation -33 0 76 + rotation 0 1 0 1.57 + name "road(16)" + width 6 + numberOfForwardLanes 2 + lines [ + RoadLine { + type "continuous" + width 0.3 + } + ] + rightBarrier TRUE + leftBarrier TRUE + bottom TRUE + length 66 + appearance Asphalt { + colorOverride 0 0 0 + IBLStrength 0 + } + pavementAppearance StonePavement { + colorOverride 0 0 0 + } + } + Solid { + translation -15 0 -48.41 + rotation 0 1 0 -1.26 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 10.67 + } + } + ] + name "solid(9)" + } + Solid { + translation 15 0 -48.39 + rotation 0 1 0 -1.88 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.01 10.67 + } + } + ] + name "solid(10)" + } + ] + name "environment" +} +Floor { + size 176 176 + tileSize 0.1 0.1 + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + transparency 0.9 + metalness 0 + } +} diff --git a/webots/controllers/controller/Makefile b/webots/controllers/controller/Makefile index 7bf806a..bcb8177 100644 --- a/webots/controllers/controller/Makefile +++ b/webots/controllers/controller/Makefile @@ -78,9 +78,15 @@ ifeq ($(UNAME_S),Linux) LIBRARIES += -lpthread -lboost_system endif +ifeq ($(UNAME_S),Darwin) # Mac OS X +LIBRARIES += -lpthread -lboost_system +#LIBRARIES += "-L/Users/luyandamlangeni/boost/boost_1_66_0/stage/lib" # -lboost_system +endif + CXX_SOURCES = controller.cpp robot_container.cpp ../../../shared/buffer_reader.cpp ../../../shared/buffer_writer.cpp ../../../shared/message.cpp -CFLAGS = -Wno-strict-aliasing -Wno-unused-variable +CFLAGS = -Wno-strict-aliasing -Wno-unused-variable -std=c++11 + ### Do not modify: this includes Webots global Makefile.include space := diff --git a/webots/controllers/controller/build/release/controller.d b/webots/controllers/controller/build/release/controller.d deleted file mode 100644 index ab34799..0000000 --- a/webots/controllers/controller/build/release/controller.d +++ /dev/null @@ -1,9 +0,0 @@ -build/release/controller.o: controller.cpp \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/Robot.hpp \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/Motor.hpp \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/Device.hpp \ - D:/Henrik/Uni/EES/Webots/include/controller/c/webots/types.h \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/Camera.hpp \ - D:/Henrik/Uni/EES/Webots/include/controller/c/webots/camera_recognition_object.h \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/DistanceSensor.hpp \ - D:\Henrik\Uni\EES\Webots/include/controller/cpp/webots/LightSensor.hpp diff --git a/webots/controllers/controller/build/release/controller.exe b/webots/controllers/controller/build/release/controller.exe deleted file mode 100644 index 73524c5..0000000 Binary files a/webots/controllers/controller/build/release/controller.exe and /dev/null differ diff --git a/webots/controllers/controller/build/release/controller.o b/webots/controllers/controller/build/release/controller.o deleted file mode 100644 index f161b29..0000000 Binary files a/webots/controllers/controller/build/release/controller.o and /dev/null differ diff --git a/webots/controllers/controller/controller b/webots/controllers/controller/controller new file mode 100755 index 0000000..891527c Binary files /dev/null and b/webots/controllers/controller/controller differ diff --git a/webots/controllers/controller/controller.cpp b/webots/controllers/controller/controller.cpp index 182ae64..6748bb3 100644 --- a/webots/controllers/controller/controller.cpp +++ b/webots/controllers/controller/controller.cpp @@ -16,7 +16,7 @@ int main(int argc, char **argv) robot_container container; container.wait_for_connection(argv[1]); - container.setup_robot(); + container.setup_robot(8); container.run(); return 0; } diff --git a/webots/controllers/controller/robot_container.cpp b/webots/controllers/controller/robot_container.cpp index 0a35401..a3b00f1 100644 --- a/webots/controllers/controller/robot_container.cpp +++ b/webots/controllers/controller/robot_container.cpp @@ -5,16 +5,18 @@ #include "robot_container.hpp" #include "../../../shared/message.hpp" -robot_container::robot_container() +robot_container::robot_container() : _external(_io_service), _reader(_in), _writer(_in) { _basic_time_step = (int) _robot.getBasicTimeStep(); + _last = std::chrono::duration_cast + (std::chrono::system_clock::now().time_since_epoch()); } -void robot_container::setup_robot() +void robot_container::setup_robot(int n_images) { std::cout << "setting up distance sensors" << '\n'; - std::vector ds_names = {"ds_fc", "ds_fcr", "ds_fr", "ds_rf", "ds_rc", + std::vector ds_names = {"ds_fc", "ds_fcr", "ds_fr", "ds_rf", "ds_rc", "ds_lc", "ds_lf", "ds_fl", "ds_fcl"}; for(std::size_t i = 0; ienable(_basic_time_step); _image_size = _camera->getHeight() * _camera->getWidth() * 4 * sizeof(unsigned char); + // limiting the number if image messages send per second + // standart value is 10 + if((n_images * 16) > 1000 || n_images == -1) + { + n_images = 10; + } + // calculate wait time in milliseconds in steps of _basic_time_step + _wait_time = static_cast(1000 / (n_images * _basic_time_step)) * _basic_time_step; + std::cout << "wait time: " << _wait_time << '\n'; + std::cout << "setting up motor" << '\n'; std::vector m_names = {"wheelFR", "wheelRR", "wheelRL", "wheelFL"}; for(std::size_t i = 0; i + (std::chrono::system_clock::now().time_since_epoch()); + // turn on camera at least _basic_time_step millieconds before _wait_time + if((now - _last).count() > (_wait_time - (_basic_time_step + 4))){ + _camera->enable(_basic_time_step); + } + + if((now - _last).count() < _wait_time){ + return; + } + // std::cout << (now - _last).count() << '\n'; + const unsigned char *image = _camera->getImage(); std::vector vec(image, image + _image_size); external_image_data_message message; message.pixel = vec; writer << message; + + _last = now; + _camera->disable(); } void robot_container::run() { - while (_robot.step(_basic_time_step) != -1) + while (_robot.step(_basic_time_step) != -1) { buffer out; buffer_writer out_writer(out); @@ -139,7 +165,7 @@ void robot_container::run() _motors[1]->setVelocity(vl_msg.right_speed); _motors[2]->setVelocity(vl_msg.left_speed); _motors[3]->setVelocity(vl_msg.left_speed); - // std::cout << "recieved rs: " << vl_msg.right_speed << ", ls: " << vl_msg.left_speed << '\n'; + std::cout << "recieved rs: " << vl_msg.right_speed << ", ls: " << vl_msg.left_speed << '\n'; } catch(const std::exception& e) { @@ -153,4 +179,4 @@ void robot_container::run() _motors[i]->setVelocity(0); } _external.close(); -} \ No newline at end of file +} diff --git a/webots/controllers/controller/robot_container.hpp b/webots/controllers/controller/robot_container.hpp index 5fe817a..894571d 100644 --- a/webots/controllers/controller/robot_container.hpp +++ b/webots/controllers/controller/robot_container.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -25,6 +26,9 @@ class robot_container std::array _light_sensors; webots::Camera *_camera; int _image_size; + int _wait_time; + + std::chrono::milliseconds _last; boost::asio::io_service _io_service; boost::asio::ip::tcp::socket _external; @@ -37,7 +41,7 @@ class robot_container void fill_image_data_message(buffer_writer& writer); public: robot_container(); - void setup_robot(); + void setup_robot(int n_images); int wait_for_connection(char *port); void run(); }; diff --git a/webots/worlds/.cabservice.wbproj b/webots/worlds/.cabservice.wbproj index 04af7ec..815d01a 100644 --- a/webots/worlds/.cabservice.wbproj +++ b/webots/worlds/.cabservice.wbproj @@ -1,7 +1,7 @@ Webots Project File version R2020a -perspectives: 000000ff00000000fd00000003000000000000020000000448fc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000007f0000040dfc0200000001fb0000001400540065007800740045006400690074006f007201000000150000040d0000008700ffffff000000030000078000000039fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000006900ffffff000006ff0000040d00000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001180000060b0100000002010000000101 -sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000002010000000201 +perspectives: 000000ff00000000fd00000003000000000000022300000294fc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770100000000000002230000006900fffffffb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000147000000a80000006900ffffff00000001000000750000024bfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000024b0000008900ffffff00000003000002e900000047fc0100000001fb0000000e0043006f006e0073006f006c00650100000225000002e90000006900ffffff000002720000024b00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000000fa000001d60100000002010000000101 +sceneTreePerspectives: 000000ff000000010000000200000114000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd00000003000000000000016c00000469fc0200000001fb00000012005300630065006e006500540072006500650100000019000004690000018500ffffff000000010000022e0000036bfc0200000001fb0000001400540065007800740045006400690074006f007201000000190000036b000000ab00ffffff00000003000005cd000000f8fc0100000001fb0000000e0043006f006e0073006f006c00650100000172000005cd0000004e00ffffff000003990000036b00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 @@ -9,7 +9,7 @@ projectionMode: PERSPECTIVE selectionDisabled: 0 viewpointLocked: 0 orthographicViewHeight: 1 -textFiles: 0 "controllers/webots_controller/webots_controller.c" -globalOptionalRendering: DistanceSensorRays -renderingDevicePerspectives: Cab1:camera;1;3.2;0;0 -renderingDevicePerspectives: Cab2:camera;1;3.2;0;0 +textFiles: 1 "controllers/controller/controller.cpp" "controllers/controller/robot_container.cpp" "../shared/buffer_writer.cpp" "../shared/buffer_reader.cpp" +robotWindow: Cab2 +globalOptionalRendering: LightSensorRays +renderingDevicePerspectives: Cab2:camera;1;5.84375;0;0 diff --git a/webots/worlds/cabservice.wbt b/webots/worlds/cabservice.wbt index eb43698..6d8135a 100644 --- a/webots/worlds/cabservice.wbt +++ b/webots/worlds/cabservice.wbt @@ -3,507 +3,20 @@ WorldInfo { basicTimeStep 16 } Viewpoint { - orientation 0.03808682417150016 0.7714041274115354 0.635204743399305 3.119688131672871 - position 6.409620980680648 139.501828724283 -82.55227138078858 + orientation -0.2289060383911112 -0.9726738869847278 0.03882699017575833 0.6725048978222644 + position 48.153231191329446 4.322658598687687 9.342321704473504 } TexturedBackground { luminosity 2 reflections FALSE } -DEF Cab1 Robot { - translation 0 0.4 -29.2 - rotation 0 1 0 -1.5723853071795864 - children [ - DEF cam_f Camera { - translation 0 0.1 1 - rotation 0 1 0 3.14 - children [ - Shape { - appearance PBRAppearance { - baseColor 0.4 0 0 - } - geometry Cylinder { - height 0.1 - radius 0.05 - } - } - ] - boundingObject Cylinder { - height 0.1 - radius 0.05 - } - physics Physics { - density 1 - } - fieldOfView 0.75 - width 18 - height 10 - far 10 - } - DEF ls_f LightSensor { - translation 0 0 1 - rotation 0 1 0 -1.57 - children [ - Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ls_front" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Cylinder { - height 0.1 - radius 0.05 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_fl DistanceSensor { - translation 0.45 0.2 1 - rotation 0 1 0 -1.047 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_fl" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_lf DistanceSensor { - translation 0.5 0.2 0.95 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_lf" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_lc DistanceSensor { - translation 0.5 0.2 0 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_lc" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_fr DistanceSensor { - translation -0.45 0.2 1 - rotation 0 1 0 -2.09 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_fr" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_rf DistanceSensor { - translation -0.5 0.2 0.95 - rotation 0 1 0 -3.14 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_rf" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_rc DistanceSensor { - translation -0.5 0.2 0 - rotation 0 1 0 -3.14 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_rc" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_fc DistanceSensor { - translation 0 0.2 1 - rotation 0 1 0 -1.57 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_fc" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_fcr DistanceSensor { - translation -0.225 0.2 1 - rotation 0 1 0 -1.833 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_fcr" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - DEF ds_fcl DistanceSensor { - translation 0.225 0.2 1 - rotation 0 1 0 -1.309 - children [ - Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - ] - name "ds_fcl" - boundingObject Shape { - appearance PBRAppearance { - } - geometry Box { - size 0.1 0.1 0.1 - } - } - physics Physics { - density 1 - } - lookupTable [ - 0 0 0 - 5 1000 0 - ] - } - Transform { - translation 0 0.5 -0.3 - rotation 1 0 0 0 - children [ - Shape { - appearance PBRAppearance { - baseColor 0.5 0.5 0.5 - transparency 0.3 - } - geometry Box { - size 0.9 0.5 1.2 - } - } - ] - } - Transform { - translation 0 0.8 -0.3 - rotation 1 0 0 0 - children [ - Shape { - appearance PBRAppearance { - baseColor 0 0 0 - } - geometry Box { - size 1 0.1 1.3 - } - } - ] - } - DEF wheelFL HingeJoint { - jointParameters HingeJointParameters { - anchor 0.55 -0.1 0.55 - } - device [ - DEF wheelFL RotationalMotor { - name "wheelFL" - maxTorque 30 - } - ] - endPoint DEF wheelFL Solid { - translation 0.55 -0.1 0.55 - rotation 0 0 1 1.5699999999999963 - children [ - DEF wheel Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.1 - radius 0.3 - } - } - ] - name "wheelFL" - boundingObject USE wheel - physics DEF physics_wheel Physics { - density 10 - } - } - } - DEF wheelRL HingeJoint { - jointParameters HingeJointParameters { - anchor 0.55 -0.1 -0.7 - } - device [ - DEF wheelRL RotationalMotor { - name "wheelRL" - maxTorque 30 - } - ] - endPoint DEF wheelRL Solid { - translation 0.55 -0.1 -0.7 - rotation 0 0 1 1.5699999999999963 - children [ - DEF wheel Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.1 - radius 0.3 - } - } - ] - name "wheelRL" - boundingObject USE wheel - physics DEF physics_wheel Physics { - density 10 - } - } - } - DEF wheelRR HingeJoint { - jointParameters HingeJointParameters { - anchor -0.55 -0.1 -0.7 - } - device [ - DEF wheelRR RotationalMotor { - name "wheelRR" - maxTorque 30 - } - ] - endPoint DEF wheelRR Solid { - translation -0.55 -0.1 -0.7 - rotation 0 0 1 1.5699999999999963 - children [ - DEF wheel Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.1 - radius 0.3 - } - } - ] - name "wheelRR" - boundingObject USE wheel - physics DEF physics_wheel Physics { - density 10 - } - } - } - DEF wheelFR HingeJoint { - jointParameters HingeJointParameters { - anchor -0.55 -0.1 0.55 - } - device [ - DEF wheelFR RotationalMotor { - name "wheelFR" - maxTorque 30 - } - ] - endPoint DEF wheelFR Solid { - translation -0.55 -0.1 0.55 - rotation 0 0 1 1.5699999999999956 - children [ - DEF wheel Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - } - geometry Cylinder { - height 0.1 - radius 0.3 - } - } - ] - name "wheelFR" - boundingObject USE wheel - physics DEF physics_wheel Physics { - density 10 - } - } - } - DEF car_body Shape { - appearance PBRAppearance { - baseColor 1 1 0 - } - geometry Box { - size 1 0.6 2 - } - } - ] - name "Cab1" - boundingObject USE car_body - physics Physics { - density 10 - } - recognitionColors [ - 0 0 0 - ] - controller "controller" -} DEF Cab2 Robot { - translation 10 0.4 -29.1 - rotation 0 1 0 -1.5723853071795864 + translation 53.08639034074971 0.39866977221676086 0.8210340586980484 + rotation -0.10807059688532915 0.9941430869624803 0.0005183951765051135 -0.0021523730941073976 children [ DEF cam_f Camera { translation 0 0.1 1 - rotation 0 1 0 3.14 + rotation 0.0001018959294375226 -0.971928669461667 -0.23527569168040463 -3.140015307179586 children [ Shape { appearance PBRAppearance { @@ -523,13 +36,12 @@ DEF Cab2 Robot { density 1 } fieldOfView 0.75 - width 18 - height 10 + height 5 far 10 } DEF ls_f LightSensor { - translation 0 0 1 - rotation 0 1 0 -1.57 + translation -0.530812 0.00071 -0.018814 + rotation 0.00028104690193075005 0.7071067532602977 0.7071067532602977 -3.141025307179586 children [ Shape { appearance PBRAppearance { @@ -557,6 +69,7 @@ DEF Cab2 Robot { 0 0 0 5 1000 0 ] + occlusion TRUE } DEF ds_fl DistanceSensor { translation 0.45 0.2 1 @@ -839,6 +352,7 @@ DEF Cab2 Robot { } DEF wheelFL HingeJoint { jointParameters HingeJointParameters { + position 2518.487501985725 anchor 0.55 -0.1 0.55 } device [ @@ -849,7 +363,7 @@ DEF Cab2 Robot { ] endPoint DEF wheelFL Solid { translation 0.55 -0.1 0.55 - rotation 0 0 1 1.5699999999999963 + rotation 0.681671629554792 -0.6811290121925502 -0.26718356650384645 3.6641597931520367 children [ DEF wheel Shape { appearance PBRAppearance { @@ -867,10 +381,13 @@ DEF Cab2 Robot { physics DEF physics_wheel Physics { density 10 } + linearVelocity -0.002558085006116612 9.343365705307473e-06 1.2000306854532534 + angularVelocity 4.000010185305242 -1.546534534814798e-05 0.008578616312256388 } } DEF wheelRL HingeJoint { jointParameters HingeJointParameters { + position 7191.815090854546 anchor 0.55 -0.1 -0.7 } device [ @@ -881,7 +398,7 @@ DEF Cab2 Robot { ] endPoint DEF wheelRL Solid { translation 0.55 -0.1 -0.7 - rotation 0 0 1 1.5699999999999963 + rotation 0.6843210583879725 -0.6837763320541612 -0.25328761669162264 3.6381128441835404 children [ DEF wheel Shape { appearance PBRAppearance { @@ -899,10 +416,13 @@ DEF Cab2 Robot { physics DEF physics_wheel Physics { density 10 } + linearVelocity -0.002595415940324201 1.2645332829935152e-05 1.200018827050314 + angularVelocity 4.0000027330992225 1.701503893577195e-07 0.008572911913928862 } } DEF wheelRR HingeJoint { jointParameters HingeJointParameters { + position 6438.8981917884885 anchor -0.55 -0.1 -0.7 } device [ @@ -913,7 +433,7 @@ DEF Cab2 Robot { ] endPoint DEF wheelRR Solid { translation -0.55 -0.1 -0.7 - rotation 0 0 1 1.5699999999999963 + rotation 0.5341966638964049 -0.5337714380660853 -0.6555318269823479 4.302831410861123 children [ DEF wheel Shape { appearance PBRAppearance { @@ -931,10 +451,13 @@ DEF Cab2 Robot { physics DEF physics_wheel Physics { density 10 } + linearVelocity -0.002594943808660431 -9.089326171472544e-06 1.199976485655985 + angularVelocity 3.9999825657958987 -5.552929463861547e-06 0.008574669039919665 } } DEF wheelFR HingeJoint { jointParameters HingeJointParameters { + position 6438.946806065444 anchor -0.55 -0.1 0.55 } device [ @@ -945,7 +468,7 @@ DEF Cab2 Robot { ] endPoint DEF wheelFR Solid { translation -0.55 -0.1 0.55 - rotation 0 0 1 1.5699999999999956 + rotation 0.5225397848190402 -0.5221238379798098 -0.6740466386641986 4.328519770256892 children [ DEF wheel Shape { appearance PBRAppearance { @@ -963,6 +486,8 @@ DEF Cab2 Robot { physics DEF physics_wheel Physics { density 10 } + linearVelocity -0.002544193729947055 -1.2392089133500373e-05 1.1999686227246273 + angularVelocity 3.999978497971207 -2.471777788141304e-05 0.008582604289942205 } } DEF car_body Shape { @@ -983,10 +508,19 @@ DEF Cab2 Robot { 0 0 0 ] controller "controller" + controllerArgs "5555" + linearVelocity -0.0025754723462867125 -7.765371970748067e-08 1.1999989678026008 + angularVelocity 2.651440975901488e-06 -3.897144445785283e-06 2.0150630291104718e-05 } DEF environment Solid { + translation -1.80485 -1.42108e-14 -1.11554 scale 1.09819 1.09819 1.09819 children [ + CeilingSpotLight { + translation 46.67 0.520008 0 + rotation 0 0 -1 -1.5707953071795862 + spotLightIntensity 50 + } DEF PickUp1 BusStop { translation 0 0 -45 rotation 0 1 0 3.14159 @@ -1106,7 +640,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1130,7 +664,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1154,7 +688,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1170,7 +704,7 @@ DEF environment Solid { } } CurvedRoadSegment { - translation 33 0 -56 + translation 33 0 -55.95 rotation 0 1 0 1.57 name "road(9)" width 6 @@ -1178,7 +712,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1202,7 +736,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1225,7 +759,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1248,7 +782,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1272,7 +806,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1292,7 +826,7 @@ DEF environment Solid { rotation 0 1 0 0.785 roadsWidth 6 startRoadsLength [ - -0.1 + 0 ] startRoadsStartLine [ "" @@ -1316,7 +850,7 @@ DEF environment Solid { name "road intersection(1)" roadsWidth 6 startRoadsLength [ - -0.1 + 0 ] startRoadsStartLine [ "" @@ -1340,7 +874,7 @@ DEF environment Solid { name "road intersection(2)" roadsWidth 6 startRoadsLength [ - -0.1 + 0 ] startRoadsStartLine [ "" @@ -1364,7 +898,7 @@ DEF environment Solid { name "road intersection(3)" roadsWidth 6 startRoadsLength [ - -0.1 + 0 ] startRoadsStartLine [ "" @@ -1390,7 +924,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1415,7 +949,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1439,7 +973,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1454,11 +988,11 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1482,7 +1016,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1506,7 +1040,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1520,7 +1054,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1544,7 +1078,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1570,7 +1104,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" @@ -1598,7 +1132,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1614,11 +1148,11 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1645,7 +1179,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" @@ -1655,6 +1189,9 @@ DEF environment Solid { rightBarrier TRUE leftBarrier TRUE bottom TRUE + startLine [ + "" + ] appearance Asphalt { colorOverride 0 0 0 IBLStrength 0 @@ -1673,7 +1210,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1688,7 +1225,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] roadBorderWidth [ @@ -1716,7 +1253,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1740,7 +1277,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] roadBorderWidth [ @@ -1768,7 +1305,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1791,7 +1328,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1816,7 +1353,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1840,7 +1377,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1855,11 +1392,11 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1884,7 +1421,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1908,7 +1445,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -1922,7 +1459,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1946,7 +1483,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -1963,7 +1500,7 @@ DEF environment Solid { } AddLanesRoadSegment { translation 20 0 53 - rotation 0 1 0 -1.57 + rotation -8.069599999950631e-07 -0.999999999993882 -3.4036599999791764e-06 1.57 name "road(28)" width 6 length 10 @@ -1972,12 +1509,15 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE leftBarrier TRUE bottom TRUE + startLine [ + "" + ] appearance Asphalt { colorOverride 0 0 0 IBLStrength 0 @@ -1985,6 +1525,9 @@ DEF environment Solid { pavementAppearance StonePavement { colorOverride 0 0 0 } + bottomTexture [ + "" + ] } Solid { translation 15.5 0 51.48 @@ -1996,7 +1539,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -2012,11 +1555,11 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2041,7 +1584,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2065,7 +1608,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 11.6 + size 0.08 0.01 11.6 } } ] @@ -2080,7 +1623,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2103,7 +1646,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2126,7 +1669,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2150,7 +1693,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2173,7 +1716,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2197,7 +1740,7 @@ DEF environment Solid { lines [ RoadLine { type "continuous" - width 0.3 + width 0.1 } ] rightBarrier TRUE @@ -2222,7 +1765,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 10.67 + size 0.08 0.01 10.67 } } ] @@ -2238,7 +1781,7 @@ DEF environment Solid { metalness 0 } geometry Box { - size 0.2 0.01 10.67 + size 0.08 0.01 10.67 } } ] @@ -2256,3 +1799,6 @@ Floor { metalness 0 } } +WoodenBox { + translation -10.0739 0.3 -30.3994 +}