Added synchronisation between camera and data collection
This commit is contained in:
wittenator
parent
5af2dd40fb
commit
5ae995ec36
|
|
@ -4,7 +4,7 @@
|
|||
<joint name="camera_joint" type="fixed">
|
||||
<parent link="chassis"/>
|
||||
<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>
|
||||
|
||||
<link name="camera_link">
|
||||
|
|
@ -45,7 +45,7 @@
|
|||
</clip>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>20</update_rate>
|
||||
<update_rate>3</update_rate>
|
||||
<visualize>false</visualize>
|
||||
</sensor>
|
||||
<sensor name="boundingbox_camera" type="boundingbox_camera">
|
||||
|
|
@ -65,7 +65,7 @@
|
|||
</clip>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>20</update_rate>
|
||||
<update_rate>3</update_rate>
|
||||
<visualize>false</visualize>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
|
|
|||
|
|
@ -6,20 +6,20 @@
|
|||
<joint name="laser_joint" type="fixed">
|
||||
<parent link="chassis"/>
|
||||
<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>
|
||||
|
||||
|
||||
<link name="laser_frame">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.04"/>
|
||||
<box size="0.105 0.1316 0.065"/>
|
||||
</geometry>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="0.04"/>
|
||||
<box size="0.105 0.1316 0.065"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
|
||||
|
|
@ -38,16 +38,16 @@
|
|||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>640</samples>
|
||||
<samples>666</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>${-90*deg_to_rad}</min_angle>
|
||||
<max_angle>${90*deg_to_rad}</max_angle>
|
||||
<min_angle>${-60*deg_to_rad}</min_angle>
|
||||
<max_angle>${60*deg_to_rad}</max_angle>
|
||||
</horizontal>
|
||||
<vertical>
|
||||
<samples>64</samples>
|
||||
<samples>108</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>${-45*deg_to_rad}</min_angle>
|
||||
<max_angle>${0*deg_to_rad}</max_angle>
|
||||
<min_angle>${-12.5*deg_to_rad}</min_angle>
|
||||
<max_angle>${12.5*deg_to_rad}</max_angle>
|
||||
</vertical>
|
||||
</scan>
|
||||
<range>
|
||||
|
|
@ -63,5 +63,10 @@
|
|||
</ray>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
</sensor>
|
||||
<visual>
|
||||
<plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
|
||||
<label>30</label>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
|
@ -30,6 +30,11 @@
|
|||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
|
||||
<gazebo>
|
||||
<static>true</static>
|
||||
</gazebo>
|
||||
|
||||
|
||||
</link>
|
||||
|
||||
<gazebo reference="chassis">
|
||||
|
|
|
|||
|
|
@ -12,45 +12,48 @@ const std::string world_name = "empty";
|
|||
const std::string model_name = "blue_cone";
|
||||
|
||||
const int timeout_ms = 10;
|
||||
const double update_rate_hz = 100.0;
|
||||
|
||||
// Offsets
|
||||
double pose_offset_x = 0.0;
|
||||
double pose_offset_y = 0.0;
|
||||
double pose_offset_z = 0.17;
|
||||
|
||||
double x = pose_offset_x;
|
||||
double y = pose_offset_y;
|
||||
double z = pose_offset_z;
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
double z = 0;
|
||||
|
||||
double roll = 0;
|
||||
double pitch = 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)
|
||||
{
|
||||
for (const auto & gz_box : _msg.annotated_box()) {
|
||||
if (gz_box.label() == 5) {
|
||||
auto box = gz_box.box();
|
||||
auto min_corner = box.min_corner();
|
||||
auto max_corner = box.max_corner();
|
||||
// save bbox data and cone pose to csv file
|
||||
std::cout << "Bounding box: " << min_corner.x() << ", " << min_corner.y() << ", " << max_corner.x() << ", " << max_corner.y() << std::endl;
|
||||
if (pos_changed)
|
||||
{
|
||||
pthread_mutex_lock(&lock);
|
||||
for (const auto & gz_box : _msg.annotated_box()) {
|
||||
if (gz_box.label() == 5) {
|
||||
auto box = gz_box.box();
|
||||
auto min_corner = box.min_corner();
|
||||
auto max_corner = box.max_corner();
|
||||
|
||||
// save cone pose to csv file
|
||||
std::cout << "Cone pose: " << x << ", " << y << ", " << z << std::endl;
|
||||
|
||||
// write to csv file
|
||||
std::ofstream outfile;
|
||||
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();
|
||||
|
||||
// write to csv file
|
||||
std::ofstream outfile;
|
||||
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()
|
||||
{
|
||||
// Connect to the Ignition Transport node
|
||||
|
|
@ -61,73 +64,65 @@ int main()
|
|||
const std::string reqtype = "ignition.msgs.Pose";
|
||||
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 max_circle_radius = 40;
|
||||
int min_angle = 60;
|
||||
int max_angle = 210;
|
||||
|
||||
// listen for bounding box message
|
||||
std::string bbox_topic = "/box";
|
||||
node.Subscribe(bbox_topic, callback_bbox);
|
||||
|
||||
for (int i = 0; i < circles; i++)
|
||||
{
|
||||
for (int j = 0; j < circle_resolution; j++)
|
||||
{
|
||||
// move cone in rotation around origin with sin and cos
|
||||
double cur_circle_radius = min_circle_radius + (max_circle_radius - min_circle_radius) * (double)i / (double)circles;
|
||||
x = cur_circle_radius * cos(j * 2 * M_PI / circle_resolution);
|
||||
y = cur_circle_radius * sin(j * 2 * M_PI / circle_resolution);
|
||||
z = 0;
|
||||
for (int i = 0; i < 1000; i++)
|
||||
{
|
||||
std::cout << "Iteration: " << i << std::endl;
|
||||
while (pos_changed)
|
||||
pthread_cond_wait (&bbox_cond, &lock);
|
||||
// random float between 1 and 40
|
||||
float cur_circle_radius = min_circle_radius + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_circle_radius - min_circle_radius)));
|
||||
// random float between min_angle and max_angle
|
||||
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
|
||||
yaw = 0;
|
||||
pitch = 0;
|
||||
roll = (double)rand() / RAND_MAX * 2 * M_PI;
|
||||
// create request message
|
||||
ignition::msgs::Vector3d vector3d_msg;
|
||||
vector3d_msg.set_x(x);
|
||||
vector3d_msg.set_y(y);
|
||||
vector3d_msg.set_z(z);
|
||||
|
||||
// create request message
|
||||
ignition::msgs::Vector3d vector3d_msg;
|
||||
vector3d_msg.set_x(x);
|
||||
vector3d_msg.set_y(y);
|
||||
vector3d_msg.set_z(z);
|
||||
ignition::msgs::Quaternion quat_msg;
|
||||
quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
|
||||
quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
|
||||
quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
|
||||
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;
|
||||
quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
|
||||
quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
|
||||
quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
|
||||
quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2));
|
||||
ignition::msgs::Pose pose_msg;
|
||||
pose_msg.set_name(model_name);
|
||||
pose_msg.mutable_position()->CopyFrom(vector3d_msg);
|
||||
pose_msg.mutable_orientation()->CopyFrom(quat_msg);
|
||||
bool result = false;
|
||||
ignition::msgs::Boolean result_msg;
|
||||
|
||||
ignition::msgs::Pose pose_msg;
|
||||
pose_msg.set_name(model_name);
|
||||
pose_msg.mutable_position()->CopyFrom(vector3d_msg);
|
||||
pose_msg.mutable_orientation()->CopyFrom(quat_msg);
|
||||
bool result;
|
||||
ignition::msgs::Boolean result_msg;
|
||||
|
||||
// submit request (blocking)
|
||||
// submit request (blocking)
|
||||
while(!result){
|
||||
result = node.Request(service, pose_msg, timeout_ms, result_msg, result);
|
||||
|
||||
if (!result)
|
||||
{
|
||||
std::cout << "Update failed" << std::endl;
|
||||
}
|
||||
|
||||
// sleep for a short duration to avoid
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
if (!result)
|
||||
{
|
||||
std::cout << "Update failed" << std::endl;
|
||||
}
|
||||
}
|
||||
pos_changed = true;
|
||||
pthread_mutex_unlock(&lock);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -188,6 +188,9 @@
|
|||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
<plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
|
||||
<label>40</label>
|
||||
</plugin>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
|
@ -196,13 +199,6 @@
|
|||
<static>true</static>
|
||||
<link name="link">
|
||||
<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">
|
||||
<geometry>
|
||||
<mesh><uri>model://assets/blue_cone.dae</uri></mesh>
|
||||
|
|
|
|||
Loading…
Reference in New Issue