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

Compare commits

base: t.korjakow:7bde3f4610ce4aff3e0ff48955c77e35f0229f0b
Branches Tags
t.korjakow:main
..
compare: t.korjakow:8882b3be326d62d748d4f90ee2647bea00f434fc
Branches Tags
t.korjakow:main

No commits in common. "7bde3f4610ce4aff3e0ff48955c77e35f0229f0b" and "8882b3be326d62d748d4f90ee2647bea00f434fc" have entirely different histories.
7bde3f4610 ... 8882b3be32

5 changed files with 97 additions and 98 deletions
Show Stats Expand all files Collapse all files

6
description/bbox_camera.xacro
View File

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

23
description/lidar.xacro
View File

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

5
description/robot_core.xacro
View File

@ -30,11 +30,6 @@
<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">

151
src/extract_cone_pos_bbox.cpp
View File

@ -12,48 +12,45 @@ 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;
double x = 0;
double y = 0;
double z = 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 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)
{
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();
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;
// 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();
}
// 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();
}
pos_changed = false;
pthread_cond_signal (&bbox_cond);
pthread_mutex_unlock(&lock);
}
}
}
int main()
{
// Connect to the Ignition Transport node
@ -64,65 +61,73 @@ 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 < 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;
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;
// 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);
// rotate cone randomly around z axis
yaw = 0;
pitch = 0;
roll = (double)rand() / RAND_MAX * 2 * M_PI;
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));
// 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::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::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));
// submit request (blocking)
while(!result){
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)
result = node.Request(service, pose_msg, timeout_ms, result_msg, result);
if (!result)
{
std::cout << "Update failed" << std::endl;
if (!result)
{
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,9 +188,6 @@
<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>
@ -199,6 +196,13 @@
<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>