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.

Sign up for GitHub

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

Jump to bottom

[UPD] Decouple LiDAR data from DR-SPAAM module #1

Merged
merged 8 commits into from
Nov 8, 2024
Merged

[UPD] Decouple LiDAR data from DR-SPAAM module #1

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
39fa8ea
[UPD] Decouple LiDAR data from DR-SPAAM module
Jiahao9 Jun 3, 2024
881cdd0
[ADD] ability to change scan_topic_name via param file
Jiahao9 Jun 6, 2024
fe9e240
[UPD] Integrate message_filters for synchronizing FollowingPosition a…
Jiahao9 Jun 11, 2024
f3ce294
[DEL] unnecessary msg and sobit follower dr_spaam files
Jiahao9 Jun 11, 2024
f6599ad
[UPD] change LegPoseArray to PoseArray
Jiahao9 Jun 11, 2024
8f578b9
[UPD] change LegPoseArray to PoseArray
Jiahao9 Jun 11, 2024
64ac7fc
[UPD] refer to 2d_lidar_person_detection
Jiahao9 Jun 11, 2024
36600a7
[UPD] add necessary param for DR-SPAAM
Jiahao9 Jun 11, 2024
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
3 changes: 1 addition & 2 deletions multiple_sensor_person_tracking/msg/LegPoseArray.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,2 @@
std_msgs/Header header
geometry_msgs/Pose[] poses
sensor_msgs/LaserScan scan
geometry_msgs/Pose[] poses
1 change: 0 additions & 1 deletion multiple_sensor_person_tracking/scripts/dr_spaam_ros.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,6 @@ def _scan_callback(self, msg):
# convert to ros msg and publish
dets_msg = detections_to_pose_array(dets_xy, dets_cls)
dets_msg.header = msg.header
dets_msg.scan = msg
self._dets_pub.publish(dets_msg)

def detections_to_pose_array(dets_xy, dets_cls):
Expand Down
17 changes: 14 additions & 3 deletions ...sensor_person_tracking/src/sobit_edu/sobit_edu_multiple_sensor_person_tracker_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ namespace multiple_sensor_person_tracking {
ros::Publisher pub_marker_;
ros::Publisher pub_obstacles_;
ros::Publisher pub_target_odom_;
ros::Subscriber sub_scan_;

std::unique_ptr<message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray>> sub_dr_spaam_;
std::unique_ptr<message_filters::Subscriber<sobits_msgs::ObjectPoseArray>> sub_ssd_;
Expand All @@ -72,6 +73,7 @@ namespace multiple_sensor_person_tracking {
PointCloud::Ptr cloud_scan_;
visualization_msgs::MarkerArrayPtr marker_array_;
multiple_sensor_person_tracking::FollowingPositionPtr following_position_;
sensor_msgs::LaserScanConstPtr scan_msg_;

tf2_ros::Buffer tfBuffer_;
boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
Expand Down Expand Up @@ -114,10 +116,12 @@ namespace multiple_sensor_person_tracking {
const std::string& target_frame,
const geometry_msgs::Point& point );

void scan_callback (
const sensor_msgs::LaserScanConstPtr &scan_msg );

void callbackPoseArray (
const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg,
const sobits_msgs::ObjectPoseArrayConstPtr &ssd_msg );

public:
virtual void onInit();
};
Expand Down Expand Up @@ -317,6 +321,11 @@ geometry_msgs::PointStamped multiple_sensor_person_tracking::SobitEduPersonTrack
return pt_transformed;
}

void multiple_sensor_person_tracking::SobitEduPersonTracker::scan_callback (const sensor_msgs::LaserScanConstPtr &scan_msg)
{
scan_msg_ = scan_msg;
}

void multiple_sensor_person_tracking::SobitEduPersonTracker::callbackPoseArray ( const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg, const sobits_msgs::ObjectPoseArrayConstPtr &ssd_msg ) {
std::cout << "\n====================================" << std::endl;
// variable initialization
Expand All @@ -329,7 +338,7 @@ void multiple_sensor_person_tracking::SobitEduPersonTracker::callbackPoseArray (

// Sensor data to TF2 conversion
try {
projector_.transformLaserScanToPointCloud( target_frame, dr_spaam_msg->scan, cloud_scan_msg, tfBuffer_ );
projector_.transformLaserScanToPointCloud( target_frame, *scan_msg_, cloud_scan_msg, tfBuffer_ );
pcl::fromROSMsg<PointT>( cloud_scan_msg, *cloud_scan_);
cloud_scan_->header.frame_id = target_frame;
} catch ( const tf2::TransformException& ex ) {
Expand Down Expand Up @@ -481,8 +490,10 @@ void multiple_sensor_person_tracking::SobitEduPersonTracker::onInit() {
cloud_scan_.reset(new PointCloud());
marker_array_.reset(new visualization_msgs::MarkerArray);
following_position_.reset( new multiple_sensor_person_tracking::FollowingPosition );
scan_msg_.reset( new sensor_msgs::LaserScan );

sub_scan_ = nh_.subscribe(pnh_.param<std::string>( "scan_topic_name", "/scan"), 1, &SobitEduPersonTracker::scan_callback, this);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Scanデータをmessage_filtersとしてsubcribeする必要があるのではないか？
@Jiahao9


target_frame_ = pnh_.param<std::string>( "target_frame", "base_footprint" );
// message_filters :
sub_dr_spaam_ .reset ( new message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray> ( nh_, pnh_.param<std::string>( "dr_spaam_topic_name", "/dr_spaam_detections" ), 1 ) );
sub_ssd_ .reset ( new message_filters::Subscriber<sobits_msgs::ObjectPoseArray> ( nh_, pnh_.param<std::string>( "ssd_topic_name", "/ssd_object_detect/object_pose" ), 1 ) );
Expand Down
16 changes: 14 additions & 2 deletions ...sor_person_tracking/src/sobit_edu/sobit_edu_multiple_sensor_person_tracker_nodelet_id.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ namespace multiple_sensor_person_tracking {
ros::Publisher pub_marker_;
ros::Publisher pub_obstacles_;
ros::Publisher pub_target_odom_;
ros::Subscriber sub_scan_;

std::unique_ptr<message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray>> sub_dr_spaam_;
std::unique_ptr<message_filters::Subscriber<sobits_msgs::ObjectPoseArray>> sub_ssd_;
Expand All @@ -76,6 +77,7 @@ namespace multiple_sensor_person_tracking {
visualization_msgs::MarkerArrayPtr marker_array_;
visualization_msgs::MarkerArrayPtr no_exists_marker_array_;
multiple_sensor_person_tracking::FollowingPositionPtr following_position_;
sensor_msgs::LaserScanConstPtr scan_msg_;

tf2_ros::Buffer tfBuffer_;
boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
Expand Down Expand Up @@ -127,6 +129,9 @@ namespace multiple_sensor_person_tracking {
const std::string& org_frame,
const std::string& target_frame,
const geometry_msgs::Point& point );

void scan_callback (
const sensor_msgs::LaserScanConstPtr &scan_msg );

void callbackPoseArray (
const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg,
Expand Down Expand Up @@ -408,6 +413,11 @@ geometry_msgs::PointStamped multiple_sensor_person_tracking::SobitEduPersonTrack
return pt_transformed;
}

void multiple_sensor_person_tracking::SobitEduPersonTracker::scan_callback (const sensor_msgs::LaserScanConstPtr &scan_msg)
{
scan_msg_ = scan_msg;
}

void multiple_sensor_person_tracking::SobitEduPersonTrackerId::callbackPoseArray ( const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg, const sobits_msgs::ObjectPoseArrayConstPtr &ssd_msg, const person_id_follow_nodelet::SOBITTargetConstPtr &id_msg ) {
std::cout << "\n====================================" << std::endl;
// variable initialization
Expand All @@ -427,7 +437,7 @@ void multiple_sensor_person_tracking::SobitEduPersonTrackerId::callbackPoseArray

// Sensor data to TF2 conversion
try {
projector_.transformLaserScanToPointCloud( target_frame, dr_spaam_msg->scan, cloud_scan_msg, tfBuffer_ );
projector_.transformLaserScanToPointCloud( target_frame, *scan_msg_, cloud_scan_msg, tfBuffer_ );
pcl::fromROSMsg<PointT>( cloud_scan_msg, *cloud_scan_);
cloud_scan_->header.frame_id = target_frame;
} catch ( const tf2::TransformException& ex ) {
Expand Down Expand Up @@ -600,8 +610,10 @@ void multiple_sensor_person_tracking::SobitEduPersonTrackerId::onInit() {
marker_array_.reset(new visualization_msgs::MarkerArray);
no_exists_marker_array_.reset(new visualization_msgs::MarkerArray);
following_position_.reset( new multiple_sensor_person_tracking::FollowingPosition );
scan_msg_.reset( new sensor_msgs::LaserScan );

sub_scan_ = nh_.subscribe(pnh_.param<std::string>( "scan_topic_name", "/scan"), 1, &SobitEduPersonTracker::scan_callback, this);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Launch上でScanのTopicを記入できるようにした方がいいかも
@Jiahao9

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

もし，基本的に/scanというトピック名が変わらないのであれば，そのままでオッケー

Copy link
Member

@Jiahao9 Jiahao9 Jun 6, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

トラッキングする前に、/scan データ前処理する可能性はなくはないので、Launchファイルに追加します．

Copy link
Member

@Jiahao9 Jiahao9 Jun 6, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

paramファイルでトピック名を変更できるようしました。
このリクエストをマージしたあと、また新しくPull をリクエストしたいと思います


target_frame_ = pnh_.param<std::string>( "target_frame", "base_footprint" );
// message_filters :
sub_dr_spaam_ .reset ( new message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray> ( nh_, pnh_.param<std::string>( "dr_spaam_topic_name", "/dr_spaam_detections" ), 1 ) );
sub_ssd_ .reset ( new message_filters::Subscriber<sobits_msgs::ObjectPoseArray> ( nh_, pnh_.param<std::string>( "ssd_topic_name", "/ssd_object_detect/object_pose" ), 1 ) );
Expand Down
16 changes: 14 additions & 2 deletions ...sensor_person_tracking/src/sobit_pro/sobit_pro_multiple_sensor_person_tracker_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ namespace multiple_sensor_person_tracking {
ros::Publisher pub_marker_;
ros::Publisher pub_obstacles_;
ros::Publisher pub_target_odom_;
ros::Subscriber sub_scan_;

std::unique_ptr<message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray>> sub_dr_spaam_;
std::unique_ptr<message_filters::Subscriber<sobits_msgs::ObjectPoseArray>> sub_ssd_;
Expand All @@ -72,6 +73,7 @@ namespace multiple_sensor_person_tracking {
PointCloud::Ptr cloud_scan_;
visualization_msgs::MarkerArrayPtr marker_array_;
multiple_sensor_person_tracking::FollowingPositionPtr following_position_;
sensor_msgs::LaserScanConstPtr scan_msg_;

tf2_ros::Buffer tfBuffer_;
boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
Expand Down Expand Up @@ -113,6 +115,9 @@ namespace multiple_sensor_person_tracking {
const std::string& org_frame,
const std::string& target_frame,
const geometry_msgs::Point& point );

void scan_callback (
const sensor_msgs::LaserScanConstPtr &scan_msg );

void callbackPoseArray (
const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg,
Expand Down Expand Up @@ -317,6 +322,11 @@ geometry_msgs::PointStamped multiple_sensor_person_tracking::SobitProPersonTrack
return pt_transformed;
}

void multiple_sensor_person_tracking::SobitProPersonTracker::scan_callback (const sensor_msgs::LaserScanConstPtr &scan_msg)
{
scan_msg_ = scan_msg;
}

bool start = true;
void multiple_sensor_person_tracking::SobitProPersonTracker::callbackPoseArray ( const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg, const sobits_msgs::ObjectPoseArrayConstPtr &ssd_msg ) {
std::cout << "\n====================================" << std::endl;
Expand All @@ -336,7 +346,7 @@ void multiple_sensor_person_tracking::SobitProPersonTracker::callbackPoseArray (

// Sensor data to TF2 conversion
try {
projector_.transformLaserScanToPointCloud( target_frame, dr_spaam_msg->scan, cloud_scan_msg, tfBuffer_ );
projector_.transformLaserScanToPointCloud( target_frame, *scan_msg_, cloud_scan_msg, tfBuffer_ );
pcl::fromROSMsg<PointT>( cloud_scan_msg, *cloud_scan_);
cloud_scan_->header.frame_id = target_frame;
} catch ( const tf2::TransformException& ex ) {
Expand Down Expand Up @@ -514,8 +524,10 @@ void multiple_sensor_person_tracking::SobitProPersonTracker::onInit() {
cloud_scan_.reset(new PointCloud());
marker_array_.reset(new visualization_msgs::MarkerArray);
following_position_.reset( new multiple_sensor_person_tracking::FollowingPosition );
scan_msg_.reset( new sensor_msgs::LaserScan );

sub_scan_ = nh_.subscribe(pnh_.param<std::string>( "scan_topic_name", "/scan"), 1, &SobitProPersonTracker::scan_callback, this);

target_frame_ = pnh_.param<std::string>( "target_frame", "base_footprint" );
// message_filters :
sub_dr_spaam_ .reset ( new message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray> ( nh_, pnh_.param<std::string>( "dr_spaam_topic_name", "/dr_spaam_detections" ), 1 ) );
sub_ssd_ .reset ( new message_filters::Subscriber<sobits_msgs::ObjectPoseArray> ( nh_, pnh_.param<std::string>( "ssd_topic_name", "/ssd_object_detect/object_pose" ), 1 ) );
Expand Down
15 changes: 14 additions & 1 deletion ...sor_person_tracking/src/sobit_pro/sobit_pro_multiple_sensor_person_tracker_nodelet_id.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ namespace multiple_sensor_person_tracking {
ros::Publisher pub_marker_;
ros::Publisher pub_obstacles_;
ros::Publisher pub_target_odom_;
ros::Subscriber sub_scan_;

std::unique_ptr<message_filters::Subscriber<multiple_sensor_person_tracking::LegPoseArray>> sub_dr_spaam_;
std::unique_ptr<message_filters::Subscriber<sobits_msgs::ObjectPoseArray>> sub_ssd_;
Expand All @@ -76,6 +77,7 @@ namespace multiple_sensor_person_tracking {
visualization_msgs::MarkerArrayPtr marker_array_;
// visualization_msgs::MarkerArrayPtr no_exists_marker_array_;
multiple_sensor_person_tracking::FollowingPositionPtr following_position_;
sensor_msgs::LaserScanConstPtr scan_msg_;

tf2_ros::Buffer tfBuffer_;
boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
Expand Down Expand Up @@ -127,6 +129,9 @@ namespace multiple_sensor_person_tracking {
const std::string& org_frame,
const std::string& target_frame,
const geometry_msgs::Point& point );

void scan_callback (
const sensor_msgs::LaserScanConstPtr &scan_msg );

void callbackPoseArray (
const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg,
Expand Down Expand Up @@ -408,6 +413,11 @@ geometry_msgs::PointStamped multiple_sensor_person_tracking::SobitProPersonTrack
return pt_transformed;
}

void multiple_sensor_person_tracking::SobitProPersonTracker::scan_callback (const sensor_msgs::LaserScanConstPtr &scan_msg)
{
scan_msg_ = scan_msg;
}

bool start_id = false;
void multiple_sensor_person_tracking::SobitProPersonTrackerId::callbackPoseArray ( const multiple_sensor_person_tracking::LegPoseArrayConstPtr &dr_spaam_msg, const sobits_msgs::ObjectPoseArrayConstPtr &ssd_msg, const person_id_follow_nodelet::SOBITTargetConstPtr &id_msg ) {
std::cout << "\n====================================" << std::endl;
Expand All @@ -433,7 +443,7 @@ void multiple_sensor_person_tracking::SobitProPersonTrackerId::callbackPoseArray

// Sensor data to TF2 conversion
try {
projector_.transformLaserScanToPointCloud( target_frame, dr_spaam_msg->scan, cloud_scan_msg, tfBuffer_ );
projector_.transformLaserScanToPointCloud( target_frame, *scan_msg_, cloud_scan_msg, tfBuffer_ );
pcl::fromROSMsg<PointT>( cloud_scan_msg, *cloud_scan_);
cloud_scan_->header.frame_id = target_frame;
} catch ( const tf2::TransformException& ex ) {
Expand Down Expand Up @@ -627,6 +637,9 @@ void multiple_sensor_person_tracking::SobitProPersonTrackerId::onInit() {
cloud_scan_.reset(new PointCloud());
marker_array_.reset(new visualization_msgs::MarkerArray);
following_position_.reset( new multiple_sensor_person_tracking::FollowingPosition );
scan_msg_.reset( new sensor_msgs::LaserScan );

sub_scan_ = nh_.subscribe(pnh_.param<std::string>( "scan_topic_name", "/scan"), 1, &SobitProPersonTracker::scan_callback, this);

target_frame_ = pnh_.param<std::string>( "target_frame", "base_footprint" );
// message_filters :
Expand Down