t.korjakow
/
sensor_sim
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added synchronisation between camera and data collection

This commit is contained in:
wittenator 2024-03-18 17:07:34 +01:00
parent 5af2dd40fb
commit 5ae995ec36
5 changed files with 98 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
description/bbox_camera.xacro
View File

@ -4,7 +4,7 @@
<joint name="camera_joint" type="fixed"> <joint name="camera_joint" type="fixed">
<parent link="chassis"/> <parent link="chassis"/>
<child link="camera_link"/> <child link="camera_link"/>
<origin xyz="-0.996 0.0696 1.04" rpy="${11.8*deg_to_rad} ${20.2*deg_to_rad} ${22.5*deg_to_rad}"/> <origin xyz="-1.0269530 0.0328365 0.9993990" rpy="0 ${9.0*deg_to_rad} ${-20.1931*deg_to_rad}"/>
</joint> </joint>
<link name="camera_link"> <link name="camera_link">
@ -45,7 +45,7 @@
</clip> </clip>
</camera> </camera>
<always_on>1</always_on> <always_on>1</always_on>
<update_rate>20</update_rate> <update_rate>3</update_rate>
<visualize>false</visualize> <visualize>false</visualize>
</sensor> </sensor>
<sensor name="boundingbox_camera" type="boundingbox_camera"> <sensor name="boundingbox_camera" type="boundingbox_camera">
@ -65,7 +65,7 @@
</clip> </clip>
</camera> </camera>
<always_on>1</always_on> <always_on>1</always_on>
<update_rate>20</update_rate> <update_rate>3</update_rate>
<visualize>false</visualize> <visualize>false</visualize>
</sensor> </sensor>
</gazebo> </gazebo>

23
description/lidar.xacro
View File

@ -6,20 +6,20 @@
<joint name="laser_joint" type="fixed"> <joint name="laser_joint" type="fixed">
<parent link="chassis"/> <parent link="chassis"/>
<child link="laser_frame"/> <child link="laser_frame"/>
<origin xyz="-0.985 0.0 ${1.08-0.024604}" rpy="0 0 0"/> <origin xyz="-0.994175 0.0 1.0387512" rpy="0 ${9.0*deg_to_rad} 0"/>
</joint> </joint>
<link name="laser_frame"> <link name="laser_frame">
<visual> <visual>
<geometry> <geometry>
<cylinder radius="0.05" length="0.04"/> <box size="0.105 0.1316 0.065"/>
</geometry> </geometry>
<material name="red"/> <material name="red"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<cylinder radius="0.05" length="0.04"/> <box size="0.105 0.1316 0.065"/>
</geometry> </geometry>
</collision> </collision>
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05"> <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
@ -38,16 +38,16 @@
<ray> <ray>
<scan> <scan>
<horizontal> <horizontal>
<samples>640</samples> <samples>666</samples>
<resolution>1</resolution> <resolution>1</resolution>
<min_angle>${-90*deg_to_rad}</min_angle> <min_angle>${-60*deg_to_rad}</min_angle>
<max_angle>${90*deg_to_rad}</max_angle> <max_angle>${60*deg_to_rad}</max_angle>
</horizontal> </horizontal>
<vertical> <vertical>
<samples>64</samples> <samples>108</samples>
<resolution>1</resolution> <resolution>1</resolution>
<min_angle>${-45*deg_to_rad}</min_angle> <min_angle>${-12.5*deg_to_rad}</min_angle>
<max_angle>${0*deg_to_rad}</max_angle> <max_angle>${12.5*deg_to_rad}</max_angle>
</vertical> </vertical>
</scan> </scan>
<range> <range>
@ -63,5 +63,10 @@
</ray> </ray>
<alwaysOn>true</alwaysOn> <alwaysOn>true</alwaysOn>
</sensor> </sensor>
<visual>
<plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
<label>30</label>
</plugin>
</visual>
</gazebo> </gazebo>
</robot> </robot>

5
description/robot_core.xacro
View File

@ -30,6 +30,11 @@
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
<gazebo>
<static>true</static>
</gazebo>
</link> </link>
<gazebo reference="chassis"> <gazebo reference="chassis">

151
src/extract_cone_pos_bbox.cpp
View File

@ -12,45 +12,48 @@ const std::string world_name = "empty";
const std::string model_name = "blue_cone"; const std::string model_name = "blue_cone";
const int timeout_ms = 10; const int timeout_ms = 10;
const double update_rate_hz = 100.0;
// Offsets double x = 0;
double pose_offset_x = 0.0; double y = 0;
double pose_offset_y = 0.0; double z = 0;
double pose_offset_z = 0.17;
double x = pose_offset_x;
double y = pose_offset_y;
double z = pose_offset_z;
double roll = 0; double roll = 0;
double pitch = 0; double pitch = 0;
double yaw = 0; double yaw = 0;
// init semaphore
pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t bbox_cond = PTHREAD_COND_INITIALIZER;
bool pos_changed = true;
void callback_bbox(const ignition::msgs::AnnotatedAxisAligned2DBox_V &_msg) void callback_bbox(const ignition::msgs::AnnotatedAxisAligned2DBox_V &_msg)
{ {
for (const auto & gz_box : _msg.annotated_box()) { if (pos_changed)
if (gz_box.label() == 5) { {
auto box = gz_box.box(); pthread_mutex_lock(&lock);
auto min_corner = box.min_corner(); for (const auto & gz_box : _msg.annotated_box()) {
auto max_corner = box.max_corner(); if (gz_box.label() == 5) {
// save bbox data and cone pose to csv file auto box = gz_box.box();
std::cout << "Bounding box: " << min_corner.x() << ", " << min_corner.y() << ", " << max_corner.x() << ", " << max_corner.y() << std::endl; auto min_corner = box.min_corner();
auto max_corner = box.max_corner();
// save cone pose to csv file // write to csv file
std::cout << "Cone pose: " << x << ", " << y << ", " << z << std::endl; std::ofstream outfile;
outfile.open("cone_pos_bbox.csv", std::ios_base::app);
// write to csv file outfile << x << "," << y << "," << z << "," << roll << "," << pitch << "," << yaw << "," << min_corner.x() << "," << min_corner.y() << "," << max_corner.x() << "," << max_corner.y() << "\n";
std::ofstream outfile; outfile.close();
outfile.open("cone_pos_bbox.csv", std::ios_base::app); }
outfile << x << "," << y << "," << z << "," << roll << "," << pitch << "," << yaw << "," << min_corner.x() << "," << min_corner.y() << "," << max_corner.x() << "," << max_corner.y() << "\n";
outfile.close();
} }
} pos_changed = false;
pthread_cond_signal (&bbox_cond);
pthread_mutex_unlock(&lock);
}
} }
int main() int main()
{ {
// Connect to the Ignition Transport node // Connect to the Ignition Transport node
@ -61,73 +64,65 @@ int main()
const std::string reqtype = "ignition.msgs.Pose"; const std::string reqtype = "ignition.msgs.Pose";
const std::string reptype = "ignition.msgs.Boolean"; const std::string reptype = "ignition.msgs.Boolean";
// Configure the update loop
auto start_time = std::chrono::steady_clock::now();
double update_period_s = 1.0 / update_rate_hz;
auto last_update_time = start_time;
// move cone in circles around the origin and rotate the cone randomly
// after setting the position, wait a short duration and listen for a bounding box message
// write current cone position and orientation and bounding box to csv file
// repeat
int circle_resolution = 100;
int circles = 100;
int min_circle_radius = 1; int min_circle_radius = 1;
int max_circle_radius = 40; int max_circle_radius = 40;
int min_angle = 60;
int max_angle = 210;
// listen for bounding box message // listen for bounding box message
std::string bbox_topic = "/box"; std::string bbox_topic = "/box";
node.Subscribe(bbox_topic, callback_bbox); node.Subscribe(bbox_topic, callback_bbox);
for (int i = 0; i < circles; i++) for (int i = 0; i < 1000; i++)
{ {
for (int j = 0; j < circle_resolution; j++) std::cout << "Iteration: " << i << std::endl;
{ while (pos_changed)
// move cone in rotation around origin with sin and cos pthread_cond_wait (&bbox_cond, &lock);
double cur_circle_radius = min_circle_radius + (max_circle_radius - min_circle_radius) * (double)i / (double)circles; // random float between 1 and 40
x = cur_circle_radius * cos(j * 2 * M_PI / circle_resolution); float cur_circle_radius = min_circle_radius + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_circle_radius - min_circle_radius)));
y = cur_circle_radius * sin(j * 2 * M_PI / circle_resolution); // random float between min_angle and max_angle
z = 0; float random_degree = min_angle + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_angle - min_angle)));
float random_radians = random_degree * M_PI / 180;
x = cur_circle_radius * cos(random_radians);
y = cur_circle_radius * sin(random_radians);
z = 0;
// rotate cone randomly around z axis
yaw = 0;
pitch = 0;
roll = (double)rand() / RAND_MAX * 2 * M_PI;
// rotate cone randomly around z axis // create request message
yaw = 0; ignition::msgs::Vector3d vector3d_msg;
pitch = 0; vector3d_msg.set_x(x);
roll = (double)rand() / RAND_MAX * 2 * M_PI; vector3d_msg.set_y(y);
vector3d_msg.set_z(z);
// create request message ignition::msgs::Quaternion quat_msg;
ignition::msgs::Vector3d vector3d_msg; quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
vector3d_msg.set_x(x); quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
vector3d_msg.set_y(y); quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
vector3d_msg.set_z(z); quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2));
ignition::msgs::Quaternion quat_msg; ignition::msgs::Pose pose_msg;
quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2)); pose_msg.set_name(model_name);
quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2)); pose_msg.mutable_position()->CopyFrom(vector3d_msg);
quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2)); pose_msg.mutable_orientation()->CopyFrom(quat_msg);
quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2)); bool result = false;
ignition::msgs::Boolean result_msg;
ignition::msgs::Pose pose_msg; // submit request (blocking)
pose_msg.set_name(model_name); while(!result){
pose_msg.mutable_position()->CopyFrom(vector3d_msg);
pose_msg.mutable_orientation()->CopyFrom(quat_msg);
bool result;
ignition::msgs::Boolean result_msg;
// submit request (blocking)
result = node.Request(service, pose_msg, timeout_ms, result_msg, result); result = node.Request(service, pose_msg, timeout_ms, result_msg, result);
if (!result) if (!result)
{ {
std::cout << "Update failed" << std::endl; std::cout << "Update failed" << std::endl;
}
// sleep for a short duration to avoid
std::this_thread::sleep_for(std::chrono::milliseconds(100));
} }
}
pos_changed = true;
pthread_mutex_unlock(&lock);
} }

10
worlds/test_world.sdf
View File

@ -188,6 +188,9 @@
<diffuse>0.8 0.8 0.8 1</diffuse> <diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular> <specular>0.8 0.8 0.8 1</specular>
</material> </material>
<plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
<label>40</label>
</plugin>
</visual> </visual>
</link> </link>
</model> </model>
@ -196,13 +199,6 @@
<static>true</static> <static>true</static>
<link name="link"> <link name="link">
<pose>0 0 0 0 0 0</pose> <pose>0 0 0 0 0 0</pose>
<collision name="collision">
<geometry>
<box>
<size>0.112 0.112 0.3</size>
</box>
</geometry>
</collision>
<visual name="visual"> <visual name="visual">
<geometry> <geometry>
<mesh><uri>model://assets/blue_cone.dae</uri></mesh> <mesh><uri>model://assets/blue_cone.dae</uri></mesh>