From caedcb335cd265e2adafb063a76e63b9b9a02bdb Mon Sep 17 00:00:00 2001 From: alex Date: Sat, 26 Dec 2020 14:47:30 -0500 Subject: [PATCH 1/2] Up TF frequency --- stdr_robot/src/stdr_robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stdr_robot/src/stdr_robot.cpp b/stdr_robot/src/stdr_robot.cpp index b1a7aca..861d01c 100644 --- a/stdr_robot/src/stdr_robot.cpp +++ b/stdr_robot/src/stdr_robot.cpp @@ -42,7 +42,7 @@ namespace stdr_robot **/ void Robot::onInit() { - float frequency = 10; + float frequency = 50; float freq_time = 1. / frequency; ros::NodeHandle n = getMTNodeHandle(); From d714e8506f4a0bca999f6f1fe5cc81670ed053a3 Mon Sep 17 00:00:00 2001 From: alex Date: Sun, 27 Dec 2020 15:45:09 -0500 Subject: [PATCH 2/2] Change sensor TF to static transform --- stdr_robot/CMakeLists.txt | 2 + stdr_robot/include/stdr_robot/stdr_robot.h | 3 ++ stdr_robot/package.xml | 1 + stdr_robot/src/stdr_robot.cpp | 56 +++++++++++++++------- 4 files changed, 45 insertions(+), 17 deletions(-) diff --git a/stdr_robot/CMakeLists.txt b/stdr_robot/CMakeLists.txt index 69dd899..de37432 100644 --- a/stdr_robot/CMakeLists.txt +++ b/stdr_robot/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS nodelet actionlib tf + tf2_ros stdr_msgs stdr_parser geometry_msgs @@ -30,6 +31,7 @@ catkin_package( nodelet actionlib tf + tf2_ros stdr_msgs stdr_parser geometry_msgs diff --git a/stdr_robot/include/stdr_robot/stdr_robot.h b/stdr_robot/include/stdr_robot/stdr_robot.h index 430ce0f..ddf6801 100644 --- a/stdr_robot/include/stdr_robot/stdr_robot.h +++ b/stdr_robot/include/stdr_robot/stdr_robot.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,8 @@ namespace stdr_robot { //!< ROS tf transform broadcaster tf::TransformBroadcaster _tfBroadcaster; + tf2_ros::StaticTransformBroadcaster _tfStaticBroadcaster; + //!< Odometry Publisher ros::Publisher _odomPublisher; diff --git a/stdr_robot/package.xml b/stdr_robot/package.xml index 1cfa5cb..6919001 100644 --- a/stdr_robot/package.xml +++ b/stdr_robot/package.xml @@ -18,6 +18,7 @@ roscpp tf + tf2_ros sensor_msgs stdr_msgs stdr_parser diff --git a/stdr_robot/src/stdr_robot.cpp b/stdr_robot/src/stdr_robot.cpp index 861d01c..acf1d90 100644 --- a/stdr_robot/src/stdr_robot.cpp +++ b/stdr_robot/src/stdr_robot.cpp @@ -42,7 +42,7 @@ namespace stdr_robot **/ void Robot::onInit() { - float frequency = 50; + float frequency = 100; float freq_time = 1. / frequency; ros::NodeHandle n = getMTNodeHandle(); @@ -187,6 +187,26 @@ namespace stdr_robot _motionControllerPtr.reset( new IdealMotionController(_currentPose, _tfBroadcaster, n, getName(), p)); } + + for (int i = 0; i < _sensors.size(); i++) { + geometry_msgs::Pose2D sensorPose = _sensors[i]->getSensorPose(); + + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = getName(); + transformStamped.child_frame_id = _sensors[i]->getFrameId(); + transformStamped.transform.translation.x = sensorPose.x; + transformStamped.transform.translation.y = sensorPose.y; + transformStamped.transform.translation.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, sensorPose.theta); + transformStamped.transform.rotation.x = quat.x(); + transformStamped.transform.rotation.y = quat.y(); + transformStamped.transform.rotation.z = quat.z(); + transformStamped.transform.rotation.w = quat.w(); + _tfStaticBroadcaster.sendTransform(transformStamped); + } _tfTimer.start(); } @@ -446,6 +466,8 @@ namespace stdr_robot { _motionControllerPtr->setPose(_previousPose); } + + ros::Time timeNow = ros::Time::now(); //!< Robot tf tf::Vector3 translation(_previousPose.x, _previousPose.y, 0); @@ -455,11 +477,11 @@ namespace stdr_robot tf::Transform mapToRobot(rotation, translation); _tfBroadcaster.sendTransform(tf::StampedTransform( - mapToRobot, ros::Time::now(), "map_static", getName())); + mapToRobot, timeNow, "map_static", getName())); //!< Odometry nav_msgs::Odometry odom; - odom.header.stamp = ros::Time::now(); + odom.header.stamp = timeNow; odom.header.frame_id = "map_static"; odom.child_frame_id = getName(); odom.pose.pose.position.x = _previousPose.x; @@ -470,23 +492,23 @@ namespace stdr_robot _odomPublisher.publish(odom); - //!< Sensors tf - for (int i = 0; i < _sensors.size(); i++) { - geometry_msgs::Pose2D sensorPose = _sensors[i]->getSensorPose(); + // //!< Sensors tf + // for (int i = 0; i < _sensors.size(); i++) { + // geometry_msgs::Pose2D sensorPose = _sensors[i]->getSensorPose(); - tf::Vector3 trans(sensorPose.x, sensorPose.y, 0); - tf::Quaternion rot; - rot.setRPY(0, 0, sensorPose.theta); + // tf::Vector3 trans(sensorPose.x, sensorPose.y, 0); + // tf::Quaternion rot; + // rot.setRPY(0, 0, sensorPose.theta); - tf::Transform robotToSensor(rot, trans); + // tf::Transform robotToSensor(rot, trans); - _tfBroadcaster.sendTransform( - tf::StampedTransform( - robotToSensor, - ros::Time::now(), - getName(), - _sensors[i]->getFrameId())); - } + // _tfBroadcaster.sendTransform( + // tf::StampedTransform( + // robotToSensor, + // ros::Time::now(), + // getName(), + // _sensors[i]->getFrameId())); + // } ROS_DEBUG_STREAM("Timing for publishing tf and odom: " << (ros::WallTime::now() - start_time).toSec() * 1000 << " ms"); }