Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mirte ROS1 simulation #2

Open
wants to merge 73 commits into
base: ROS1
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
73 commits
Select commit Hold shift + click to select a range
0a93627
Fixes for ROS1 first launch
ArendJan Jun 28, 2023
b8f5b98
Teleop working
ArendJan Jun 29, 2023
7e9d212
Add duckytown generation + start sim
ArendJan Jul 19, 2023
00e1cc2
xmllint install workflow
ArendJan Jul 19, 2023
fbe93a1
change to tidy
ArendJan Jul 19, 2023
857ee7f
add format information readme
ArendJan Jul 19, 2023
49c29e5
fix workflow
ArendJan Jul 19, 2023
43f5cb7
fixes, but not working yet
ArendJan Jul 19, 2023
cbe4085
working control
ArendJan Jul 19, 2023
0351eaf
Add lidar
ArendJan Jul 19, 2023
8934bd8
sonar wip
ArendJan Jul 20, 2023
b52dbca
add sonar + fix formatting
ArendJan Jul 20, 2023
9c51b65
Add wheel encoders
ArendJan Jul 20, 2023
a341d1e
add encoder_speed
ArendJan Jul 20, 2023
cea2f75
Add intensity sensor
ArendJan Jul 21, 2023
0599291
Fix python styling map IR & Encoder
ArendJan Jul 21, 2023
4ff1e00
Add launch arguments
ArendJan Jul 21, 2023
46e8a06
Add encoder arguments
ArendJan Jul 21, 2023
d634699
Enable all and change lidar min_distance
ArendJan Jul 21, 2023
551ec0e
Add duckietown as model vs robot
ArendJan Jul 21, 2023
1a14ac6
World spawning done
ArendJan Jul 26, 2023
7bb9f4c
Add duckietown models!
ArendJan Jul 26, 2023
14f2194
Fix stylings
ArendJan Jul 26, 2023
6544282
Improve README and format worlds
ArendJan Jul 27, 2023
e29b44e
Add realsense_rs200
ArendJan Aug 3, 2023
5461096
Add realsense dependency comment
ArendJan Aug 3, 2023
4ad7cbf
Changed dimensions to move wheels, stylecheck yaml, fix realsense vis…
ArendJan Aug 3, 2023
9e185dc
New mirte_base and collision stl
ArendJan Aug 3, 2023
260df6d
Add lidar stl on top
ArendJan Aug 3, 2023
a3c2e52
Fix lidar styling
ArendJan Aug 3, 2023
3e230ea
Fix mirte not moving heavy caster
ArendJan Aug 3, 2023
193f188
start RCJ world
ArendJan Aug 3, 2023
d0f02a1
start rsp
ArendJan Aug 4, 2023
43798a8
revert lidar + add duckie material
ArendJan Aug 8, 2023
366b5f8
first working rsp
ArendJan Aug 9, 2023
bed8adf
working with lockstep
ArendJan Aug 9, 2023
36477a1
done rsp lab 4
ArendJan Aug 10, 2023
d164a28
Convert mapping to cpp
ArendJan Aug 11, 2023
46e46bc
change to other diff drive
ArendJan Aug 11, 2023
5810bcb
fix compile
ArendJan Aug 15, 2023
d2cdaba
fix compile + add generated world
ArendJan Aug 15, 2023
1937d25
Add lab 4 generated world
ArendJan Aug 15, 2023
f0ce9c7
Add lab4 launch file
ArendJan Aug 15, 2023
4d00d7d
Wider 'realsense' + fixes rsp
ArendJan Aug 15, 2023
1d7fcc6
Fix lab4 person detection
ArendJan Aug 16, 2023
26cb3fc
Add black tile rsp lab3
ArendJan Aug 16, 2023
d25adac
Merge branch 'rsp' of https://github.com/ArendJan/mirte-gazebo into rsp
ArendJan Aug 16, 2023
d93c231
cleanup of worlds and launch files
ArendJan Aug 16, 2023
5c4895f
Add lab4 launch file
ArendJan Aug 16, 2023
c66e176
cleanup empty duckietown
ArendJan Aug 16, 2023
1bc16fb
Stylecheck fixes
ArendJan Aug 16, 2023
914c387
Change sky color for person detection
ArendJan Aug 16, 2023
18c09c7
simplify stls for faster rendering
ArendJan Aug 16, 2023
2d8b414
Fix add joint controller for state publishing
ArendJan Aug 17, 2023
41a96b5
Convert encoder converter from python to cpp
ArendJan Aug 17, 2023
49370e5
Fix styling
ArendJan Aug 17, 2023
d1aafc0
Copy over realsense model to remove need for realsense_gazebo_plugin
ArendJan Aug 17, 2023
c3b866e
Fix spawn error because of sim time
ArendJan Aug 17, 2023
c4e4aad
Cleanup and buildMap fix
ArendJan Aug 17, 2023
5c87116
Cleanup and rosdep python-yaml
ArendJan Aug 17, 2023
8c57c93
Add cpp styling workflow and fixes
ArendJan Aug 17, 2023
078cb95
remove prius_msgs
ArendJan Aug 17, 2023
7710039
remove pcl_ros
ArendJan Aug 17, 2023
016003f
Add pcl dependency
ArendJan Aug 17, 2023
e1d62a8
Remove unnecesary code
ArendJan Aug 17, 2023
7cea82c
Remove unnecesary function
ArendJan Aug 17, 2023
6bf2cfb
add gazebo gui param labs
ArendJan Aug 17, 2023
3579406
Fix cpp styling
ArendJan Aug 18, 2023
6383097
change perspective gazebo user_camera
ArendJan Aug 18, 2023
82e11c2
Change camera to show overview
ArendJan Aug 18, 2023
cf66e2e
Add base_pose_ground_truth
ArendJan Aug 22, 2023
b7ccc7c
Add collision sensor mirte & realsense
ArendJan Sep 1, 2023
9681222
fix topics
ArendJan Sep 1, 2023
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add cpp styling workflow and fixes
ArendJan committed Aug 17, 2023
commit 8c57c93e9586b12b0313320dda9055ef4ccb43e4
17 changes: 17 additions & 0 deletions .github/workflows/cpp-check.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
name: test-clang-format

on: [push, pull_request]

jobs:
cpp_style_check:
runs-on: ubuntu-latest

steps:
- uses: actions/checkout@v3
- uses: DoozyX/clang-format-lint-action@v0.14
with:
source: '.'
# exclude: './third_party ./external'
extensions: 'h,cpp'
clangFormatVersion: 14
style: llvm
11 changes: 7 additions & 4 deletions src/convert_encoder.cpp
Original file line number Diff line number Diff line change
@@ -52,7 +52,8 @@ class map_encoder {
} else {
auto diff = ticks - this->last_actual_ticks;
this->last_actual_ticks = ticks;
mess.value = this->last_published_ticks + abs(diff); // always increase the ticks
mess.value =
this->last_published_ticks + abs(diff); // always increase the ticks
}
this->last_published_ticks = mess.value;
this->pub_.publish(mess);
@@ -68,15 +69,17 @@ class map_encoder {
static int last_tick_count = 0;
mirte_msgs::Encoder mess;
mess.value = this->last_published_ticks - last_tick_count;
if(!this->bidirectional) {
mess.value = abs(mess.value);
if (!this->bidirectional) {
mess.value = abs(mess.value);
}
mess.header = this->last_header;
pub_speed_.publish(mess);
last_tick_count = this->last_published_ticks;
}

int map_rad_to_ticks(double ang) { return (int)(ang / (2 * M_PI) * this->ticks_per_rot); }
int map_rad_to_ticks(double ang) {
return (int)(ang / (2 * M_PI) * this->ticks_per_rot);
}

private:
// Create the necessary objects for subscribing and publishing
2 changes: 1 addition & 1 deletion src/convert_ultrasonic.cpp
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
#include <algorithm>
#include <iostream>

#include <mirte_msgs/GetDistance.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <algorithm>

class map_sonar {

125 changes: 63 additions & 62 deletions src/rotate.cpp
Original file line number Diff line number Diff line change
@@ -1,36 +1,32 @@
#include <ros/ros.h>


#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <prius_msgs/Control.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <vision_msgs/Detection3DArray.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>

class pcl_rotate {

public:
void removePlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
void removePlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// std::cout << "planeStart" << cloud->size() << std::endl;
@@ -96,39 +92,41 @@ class pcl_rotate {

return clusters;
}
vision_msgs::BoundingBox3D toBoundingBox(pcl::PointCloud<pcl::PointXYZ> const & cloud) {
vision_msgs::BoundingBox3D result;
if (cloud.empty()) { return result; }
// Computes centroid of the cluster
Eigen::Vector4f centroid;

pcl::compute3DCentroid(cloud, centroid);
vision_msgs::BoundingBox3D
toBoundingBox(pcl::PointCloud<pcl::PointXYZ> const &cloud) {
vision_msgs::BoundingBox3D result;
if (cloud.empty()) {
return result;
}
// Computes centroid of the cluster
Eigen::Vector4f centroid;

result.center.position.x = centroid[0];
result.center.position.y = centroid[1];
result.center.position.z = centroid[2];
pcl::compute3DCentroid(cloud, centroid);

pcl::PointXYZ min, max;
pcl::getMinMax3D(cloud, min, max);
result.center.position.x = centroid[0];
result.center.position.y = centroid[1];
result.center.position.z = centroid[2];

result.size.x = max.x - min.x;
result.size.y = max.y - min.y;
result.size.z = max.z - min.z;
pcl::PointXYZ min, max;
pcl::getMinMax3D(cloud, min, max);

return result;
}
result.size.x = max.x - min.x;
result.size.y = max.y - min.y;
result.size.z = max.z - min.z;

return result;
}

pcl_rotate(ros::NodeHandle nh) {
// Topic you publish to
pub_ = n_.advertise<sensor_msgs::PointCloud2>(
"/mirte/depth_cam/pointsRot", 1000);
pub_ = n_.advertise<sensor_msgs::PointCloud2>("/mirte/depth_cam/pointsRot",
1000);

// Topic you subscribe to
sub_ = n_.subscribe("/mirte/depth_cam/points", 1000,
&pcl_rotate::callback, this);
sub_ = n_.subscribe("/mirte/depth_cam/points", 1000, &pcl_rotate::callback,
this);
}

void callback(const sensor_msgs::PointCloud2 &input_cloud) {
vision_msgs::Detection3DArray detectionArray;
detectionArray.header =
@@ -139,37 +137,40 @@ vision_msgs::BoundingBox3D toBoundingBox(pcl::PointCloud<pcl::PointXYZ> const &
// cloud with the PointCloud format
// std::cout << __LINE__ << std::endl;
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.8);
//pass.setNegative (true);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 1.8);
// pass.setNegative (true);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(
new pcl::PointCloud<pcl::PointXYZ>);

pass.filter (*cloud_filtered);
pass.filter(*cloud_filtered);

this->removeNan(cloud_filtered);
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

float theta = -M_PI/2;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
transform_2.rotate (Eigen::AngleAxisf (-theta, Eigen::Vector3f::UnitY()));

// Print the transformation
// printf ("\nMethod #2: using an Affine3f\n");
// std::cout << transform_2.matrix() << std::endl;

// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*cloud_filtered, *transformed_cloud, transform_2);
sensor_msgs::PointCloud2 out_cloud;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

float theta = -M_PI / 2;
transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX()));
transform_2.rotate(Eigen::AngleAxisf(-theta, Eigen::Vector3f::UnitY()));

// Print the transformation
// printf ("\nMethod #2: using an Affine3f\n");
// std::cout << transform_2.matrix() << std::endl;

// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(
new pcl::PointCloud<pcl::PointXYZ>());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud(*cloud_filtered, *transformed_cloud, transform_2);
sensor_msgs::PointCloud2 out_cloud;
pcl::toROSMsg(*transformed_cloud, out_cloud);

pub_.publish(out_cloud);
}
void removeNan(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
cloud->is_dense = false;
// std::cerr << "orig cloud data: " << cloud->size() << " points" << std::endl;
// std::cerr << "orig cloud data: " << cloud->size() << " points" <<
// std::endl;

boost::shared_ptr<std::vector<int>> indices(new std::vector<int>);
pcl::removeNaNFromPointCloud(*cloud, *indices);
@@ -187,18 +188,18 @@ vision_msgs::BoundingBox3D toBoundingBox(pcl::PointCloud<pcl::PointXYZ> const &
private:
// Create the necessary objects for subscribing and publishing
ros::NodeHandle n_;
ros::Publisher pub_; ros::Publisher pub_2;
ros::Publisher pub_;
ros::Publisher pub_2;

ros::Subscriber sub_;
}; // End of class SubscribeAndPublish


int main (int argc, char **argv)
{
int main(int argc, char **argv) {
ros::init(argc, argv, "pcl_pointcloud_rotate");
ros::NodeHandle nh;

//Create an object of class pcl_object_detector_sub_and_pub that will take care of everything
// Create an object of class pcl_object_detector_sub_and_pub that will take
// care of everything
pcl_rotate pcl_object(nh);
ros::spin();
return 0;