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

Static TF Transform #1

Open
wants to merge 2 commits into
base: merged_bumper
Choose a base branch
from
Open

Static TF Transform #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
caedcb3
Up TF frequency
Dec 26, 2020
d714e85
Change sensor TF to static transform
Dec 27, 2020
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
2 changes: 2 additions & 0 deletions stdr_robot/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
nodelet
actionlib
tf
tf2_ros
stdr_msgs
stdr_parser
geometry_msgs
Expand All @@ -30,6 +31,7 @@ catkin_package(
nodelet
actionlib
tf
tf2_ros
stdr_msgs
stdr_parser
geometry_msgs
Expand Down
3 changes: 3 additions & 0 deletions stdr_robot/include/stdr_robot/stdr_robot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <stdr_msgs/RobotMsg.h>
#include <stdr_msgs/MoveRobot.h>
#include <stdr_robot/sensors/sensor_base.h>
Expand Down Expand Up @@ -153,6 +154,8 @@ namespace stdr_robot {
//!< ROS tf transform broadcaster
tf::TransformBroadcaster _tfBroadcaster;

tf2_ros::StaticTransformBroadcaster _tfStaticBroadcaster;

//!< Odometry Publisher
ros::Publisher _odomPublisher;

Expand Down
1 change: 1 addition & 0 deletions stdr_robot/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

<depend>roscpp</depend>
<depend>tf</depend>
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
<depend>stdr_msgs</depend>
<depend>stdr_parser</depend>
Expand Down
56 changes: 39 additions & 17 deletions stdr_robot/src/stdr_robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace stdr_robot
**/
void Robot::onInit()
{
float frequency = 10;
float frequency = 100;
float freq_time = 1. / frequency;

ros::NodeHandle n = getMTNodeHandle();
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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");
}
Expand Down