2024.5.0 (#356)

* Fixes misidentification of flipped parts on kit trays (#349) * Fix to flip issue * Fixed issue with part rotation on kitting trays * add optical frames for all rgb and rgbd cameras (#350) * Added option to build package when saving configuration file (#351) * Added option to build package when saving configuration file * Fixed priority not being written to file corrrectly * Added try and except around build command code * Update kit tray model.sdf (#353) * Updated versions (#357) --------- Co-authored-by: Joe Fernandez <[email protected]>
usnistgov · Jun 10, 2024 · 1fafedd · 1fafedd
1 parent a4927c5
commit 1fafedd
Show file tree

Hide file tree

Showing 14 changed files with 73 additions and 22 deletions.
diff --git a/ariac_controllers/package.xml b/ariac_controllers/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_controllers</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>ros2 controllers for ARIAC simulation</description>
   <maintainer email="[email protected]"> Justin Albrecht</maintainer>
   <license>NIST</license>

diff --git a/ariac_description/package.xml b/ariac_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_description</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>Package containing URDF descriptions for all robots in the ARIAC simulation</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

diff --git a/ariac_description/urdf/vacuum_gripper/vacuum_gripper_macro.xacro b/ariac_description/urdf/vacuum_gripper/vacuum_gripper_macro.xacro
@@ -107,6 +107,9 @@
         </visual>
       </link>
 
+      <link name ="${prefix}_robot_camera_optical_link">
+      </link>
+
       <!-- Add camera mount joint -->
       <joint name="${prefix}_robot_camera_mount_joint" type="fixed">
         <axis xyz="0 1 0" />
@@ -123,6 +126,14 @@
         <child link="${prefix}_robot_camera_link"/>
       </joint>
 
+      <!-- Add optical joint -->
+      <joint name="${prefix}_robot_camera_optical_joint" type="fixed">
+        <axis xyz="0 1 0" />
+        <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
+        <parent link="${prefix}_robot_camera_link"/>
+        <child link="${prefix}_robot_camera_optical_link"/>
+      </joint>
+
       <!-- Add color to links -->
       <gazebo reference="${prefix}_robot_camera_mount_link">
         <material>Gazebo/Black</material>

diff --git a/ariac_gazebo/models/kit_tray/model.sdf b/ariac_gazebo/models/kit_tray/model.sdf
@@ -37,8 +37,8 @@
           </mesh>
         </geometry>
         <material>
-          <ambient>0.1 0.1 0.1 1.0</ambient>
-          <diffuse>0.1 0.1 0.1 1.0</diffuse>
+          <ambient>0.4 0.4 0.4 1.0</ambient>
+          <diffuse>0.4 0.4 0.4 1.0</diffuse>
         </material>
       </visual>
 
@@ -75,4 +75,4 @@
       </collision>
     </link>
   </model>
-</sdf>
+</sdf>
diff --git a/ariac_gazebo/nodes/sensor_tf_broadcaster.py b/ariac_gazebo/nodes/sensor_tf_broadcaster.py
@@ -40,6 +40,20 @@ def main():
 
         sensor_tf_broadcaster.generate_transform('world', sensor_name + "_frame", pose)
 
+        # Publish optical frame for cameras
+        try:
+            if 'rgb' in sensors[sensor_name]['type']:
+                xyz = [0, 0, 0]
+                rpy = ['-pi/2', 0, '-pi/2']
+
+                optical_pose = pose_info(xyz, rpy)
+
+                sensor_tf_broadcaster.generate_transform(
+                    sensor_name + "_frame", sensor_name + "_optical_frame", optical_pose)
+
+        except KeyError:
+            pass
+
     # Send tf transforms
     sensor_tf_broadcaster.send_transforms()
 

diff --git a/ariac_gazebo/package.xml b/ariac_gazebo/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_gazebo</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>ARIAC ROS2 port</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

diff --git a/ariac_gui/ariac_gui/trial_generator.py b/ariac_gui/ariac_gui/trial_generator.py
@@ -21,6 +21,8 @@
 import yaml
 from copy import copy, deepcopy
 from datetime import datetime
+import subprocess
+from time import sleep
 
 from ariac_msgs.msg import (
     Part as PartMsg,
@@ -410,17 +412,22 @@ def __init__(self):
         self.notebook.add(self.map_frame, text="Full Map")
         self.notebook_frames.append(self.map_frame)
 
-         # Map elements
+        # Map elements
         self.map_canvas_bin_elements = []
         self.map_canvas_conveyor_elements = []
         self.map_canvas_conveyor_lines = []
         self.map_canvas_assembly_station_elements = []
         self.map_canvas_agv_elements = []
         self.bin_parts_counter.trace_add('write',self.add_bin_parts_to_map)
         self.add_map_to_frame()
+
+        # Build package variable
+        self.build_package_var = ctk.StringVar()
+        self.build_package_var.set("1")
 
         self.save_file_button = ctk.CTkButton(self, text="Save file", command=self.choose_save_location, state = tk.DISABLED)
         ToolTip(self.save_file_button, msg = self.update_save_file_msg, delay=0.2)
+        self.build_package_cb = ctk.CTkCheckBox(self, text="Build ariac_gazebo pkg after saving",variable=self.build_package_var, onvalue="1", offvalue="0", height=1, width=20)
         self.light_dark_button = ctk.CTkButton(self, text="", image=ctk.CTkImage(MENU_IMAGES["dark_icon"],size=(50,50)), command = self.switch_light_dark, fg_color="#ebebeb", bg_color="#ebebeb", hover_color="#ebebeb")
         self._build_assembly_parts_pose_direction()
 
@@ -444,8 +451,9 @@ def open_main_window(self):
         self.initial_label.grid_forget()
         self.load_file_button.grid_forget()
         self.new_file_button.grid_forget()
-        self.notebook.grid(pady=10,column=LEFT_COLUMN, columnspan=2,sticky=tk.E+tk.W+tk.N+tk.S)
-        self.save_file_button.grid(pady=10,column=MIDDLE_COLUMN,row=4)
+        self.notebook.grid(pady=10,column=LEFT_COLUMN, columnspan=3,sticky=tk.E+tk.W+tk.N+tk.S)
+        self.save_file_button.grid(pady=10,column=RIGHT_COLUMN,row=4)
+        self.build_package_cb.grid(pady=10, column=MIDDLE_COLUMN, row=4)
         self.light_dark_button.grid(pady=10, column=LEFT_COLUMN, row=4)
 
 
@@ -2250,7 +2258,7 @@ def orders_to_dict(self):
                 temp_order_dict["id"] = order.id
                 temp_order_dict["type"] = ORDER_TYPES[order.type]
                 temp_order_dict["announcement"] = self.announcement_to_dict(order.condition)
-                temp_order_dict["priority"] = order.priority == "1"
+                temp_order_dict["priority"] = order.priority == True
                 if order.type == 0:
                     temp_order_dict["kitting_task"] = {}
                     temp_order_dict["kitting_task"]["agv_number"] = order.kitting_task.agv_number
@@ -3024,8 +3032,22 @@ def save_file(self, original_trial_name = False):
                 f.write(f"# CHALLENGES INFORMATION\n")
                 challenges_data = yaml.dump(self.challenges_dict,sort_keys=False,Dumper=NoAliasDumper)
                 f.write(f"\n{challenges_data}\n")
+
+        self.build_package = self.build_package_var.get() == "1"
+
         self.destroy()
-
+
+        if self.build_package:
+            try:
+                ws_path = os.path.join(*[str(item) for item in get_package_prefix("ariac_gazebo").split("/")[:-2]])
+
+                os.chdir("/" + ws_path)
+
+                build_cmd = "colcon build --packages-select ariac_gazebo"
+                subprocess.run(build_cmd, shell=True)
+            except:
+                print("ERROR: Workspace not able to be found. Cannot build.")
+
     # =======================================================
     #                    Current File
     # =======================================================
@@ -3438,4 +3460,4 @@ def update_save_file_msg(self):
             msg = "There must be at least one order to save"
         else:
             msg = f"There {'is' if num_orders == 1 else 'are'} currently {num_orders} order{'' if num_orders == 1 else 's'}"
-        return msg
+        return msg
diff --git a/ariac_gui/package.xml b/ariac_gui/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_gui</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>GUI for creating trial configurations</description>
   <maintainer email="[email protected]">Joseph Fernandez</maintainer>
   <license>NIST</license>

diff --git a/ariac_gui/setup.py b/ariac_gui/setup.py
@@ -6,7 +6,7 @@
 file_names = ['plus','assembly_station','agv','tray','light_icon','dark_icon']+[color+pType for color in PART_COLORS for pType in PART_TYPES]+[f'id_0{i}' for i in range(10)]
 setup(
     name=package_name,
-    version='2024.2.0',
+    version='2024.5.0',
     packages=[package_name],
     data_files=[
         ('share/ament_index/resource_index/packages',

diff --git a/ariac_moveit_config/package.xml b/ariac_moveit_config/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_moveit_config</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description> An package with MoveIt2 configurations for ARIAC robots</description>
   <maintainer email="[email protected]">Justin Albrecht</maintainer>
   <license>NIST</license>

diff --git a/ariac_msgs/package.xml b/ariac_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_msgs</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>Interface definitions for ARIAC</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

diff --git a/ariac_plugins/include/ariac_plugins/ariac_common.hpp b/ariac_plugins/include/ariac_plugins/ariac_common.hpp
@@ -2289,15 +2289,19 @@ namespace ariac_common
          */
         bool isFlipped()
         {
-            KDL::Frame part_on_tray;
-            tf2::fromMsg(pose_on_tray_, part_on_tray);
-            KDL::Vector part_z = part_on_tray * KDL::Vector(0, 0, 1);
+            KDL::Rotation part_rotation = KDL::Rotation::Quaternion(
+                pose_on_tray_.orientation.x,
+                pose_on_tray_.orientation.y,
+                pose_on_tray_.orientation.z,
+                pose_on_tray_.orientation.w);
+
+            KDL::Vector part_z = KDL::Frame(part_rotation) * KDL::Vector(0, 0, 1);
 
             // Calculate the angle between the two vectors
             double angle = KDL::acos(KDL::dot(KDL::Vector(0, 0, 1), part_z) / (part_z.Norm()));
 
             // Return that the part is flipped if angle is greater than ~10deg
-            if (angle > -0.2 && angle < 0.2)
+            if (angle > -0.20 && angle < 0.20)
             {
                 return false;
             }

diff --git a/ariac_plugins/package.xml b/ariac_plugins/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_plugins</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>Gazebo/ROS2 plugins for the ARAIC competition</description>
   <maintainer email="[email protected]">justin albrecht</maintainer>
   <maintainer email="[email protected]">zeid kootbally</maintainer>

diff --git a/ariac_sensors/package.xml b/ariac_sensors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ariac_sensors</name>
-  <version>2024.4.1</version>
+  <version>2024.5.0</version>
   <description>Gazebo plugins for the ARIAC sensors</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>