2024.2.0 (#313)

* add headers to basic and logical camera images

* 2024.2.0

* switch to using ros clock instead since LastMeasurement time is always 0

.gitignore

@@ -50,3 +50,6 @@ log/
 # ARIAC Logs
 /ariac_log/*/
 
+# New trials
+/ariac_gazebo/config/trials/*.yaml
+

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.1.1</version>
+  <version>2024.2.0</version>
   <description>ros2 controllers for ARIAC simulation</description>
   <maintainer email="[email protected]"> Justin Albrecht</maintainer>
   <license>NIST</license>

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.1.1</version>
+  <version>2024.2.0</version>
   <description>Package containing URDF descriptions for all robots in the ARIAC simulation</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

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.1.1</version>
+  <version>2024.2.0</version>
   <description>ARIAC ROS2 port</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

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.1.1</version>
+  <version>2024.2.0</version>
   <description>GUI for creating trial configurations</description>
   <maintainer email="[email protected]">Joseph Fernandez</maintainer>
   <license>NIST</license>

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.1.0',
+    version='2024.2.0',
     packages=[package_name],
     data_files=[
         ('share/ament_index/resource_index/packages',

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.1.1</version>
+  <version>2024.2.0</version>
   <description> An package with MoveIt2 configurations for ARIAC robots</description>
   <maintainer email="[email protected]">Justin Albrecht</maintainer>
   <license>NIST</license>

ariac_msgs/msg/AdvancedLogicalCameraImage.msg

@@ -1,3 +1,4 @@
+std_msgs/Header header
 ariac_msgs/PartPose[] part_poses
 ariac_msgs/KitTrayPose[] tray_poses
 geometry_msgs/Pose sensor_pose

ariac_msgs/msg/BasicLogicalCameraImage.msg

@@ -1,3 +1,4 @@
+std_msgs/Header header
 geometry_msgs/Pose[] part_poses
 geometry_msgs/Pose[] tray_poses
 geometry_msgs/Pose sensor_pose

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.1.1</version>
+  <version>2024.2.0</version>
   <description>Interface definitions for ARIAC</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

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.1.1</version>
+  <version>2024.2.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>

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.1.1</version>
+  <version>2024.2.0</version>
   <description>Gazebo plugins for the ARIAC sensors</description>
   <maintainer email="[email protected]">justin</maintainer>
   <license>NIST</license>

ariac_sensors/src/ariac_logical_camera_plugin.cpp

@@ -43,6 +43,7 @@ class AriacLogicalCameraPluginPrivate
 
   std::string camera_name_;
   std::string sensor_type_;
+  std::string frame_name_;
 
   std::map<std::string, int> part_types_;
   std::map<std::string, int> part_colors_;
@@ -91,6 +92,8 @@ void AriacLogicalCameraPlugin::Load(gazebo::sensors::SensorPtr _sensor, sdf::Ele
   impl_->camera_name_ = _sdf->Get<std::string>("camera_name");
   impl_->sensor_type_ = _sdf->Get<std::string>("sensor_type");
 
+  impl_->frame_name_ = gazebo_ros::SensorFrameID(*_sensor, *_sdf);
+
   if (impl_->sensor_type_ == "basic") {
     impl_->basic_pub_ = impl_->ros_node_->create_publisher<ariac_msgs::msg::BasicLogicalCameraImage>(
       "ariac/sensors/" + impl_->camera_name_ + "/image", rclcpp::SensorDataQoS());
@@ -177,6 +180,9 @@ void AriacLogicalCameraPluginPrivate::OnUpdate()
       basic_image_msg_->tray_poses.push_back(tray.pose);
     }
 
+    basic_image_msg_->header.stamp = ros_node_->get_clock()->now();
+    basic_image_msg_->header.frame_id = frame_name_;
+
     basic_pub_->publish(*basic_image_msg_);
 
   } else if (sensor_type_ == "advanced") {
@@ -185,6 +191,9 @@ void AriacLogicalCameraPluginPrivate::OnUpdate()
     advanced_image_msg_->part_poses = parts;
     advanced_image_msg_->tray_poses = trays;
 
+    advanced_image_msg_->header.stamp = ros_node_->get_clock()->now();
+    advanced_image_msg_->header.frame_id = frame_name_;
+
     advanced_pub_->publish(*advanced_image_msg_);
   }
 }