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

Publish sensors tfs as static, and allow configuring odom frequency #202

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

Publish sensors tfs as static, and allow configuring odom frequency #202

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
44b86f3
Publish sensors tfs as static, and allow configuring odom frequency
corot Nov 11, 2017
20ee82a
Implement requested changes on PR
corot Nov 13, 2017
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
4 changes: 4 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 @@ -150,6 +151,9 @@ namespace stdr_robot {
//!< ROS tf transform broadcaster
tf::TransformBroadcaster _tfBroadcaster;

//!< ROS tf2 static transform broadcaster
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
52 changes: 28 additions & 24 deletions stdr_robot/src/stdr_robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,9 +60,9 @@ namespace stdr_robot
_moveRobotService = n.advertiseService(
getName() + "/replace", &Robot::moveRobotCallback, this);

//we should not start the timer, until we hame a motion controller
//we should not start the timer, until we have a motion controller
_tfTimer = n.createTimer(
ros::Duration(0.1), &Robot::publishTransforms, this, false, false);
ros::Duration(0.01), &Robot::publishTransforms, this, false, false);
}

/**
Expand Down Expand Up @@ -172,6 +172,27 @@ namespace stdr_robot
new IdealMotionController(_currentPose, _tfBroadcaster, n, getName(), p));
}

//!< Sensors static tfs
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 @@ -407,7 +428,7 @@ namespace stdr_robot
}

/**
@brief Publishes the tf transforms every with 10Hz
@brief Publishes the tf transforms every with 100Hz
@return void
**/
void Robot::publishTransforms(const ros::TimerEvent&)
Expand All @@ -422,18 +443,19 @@ namespace stdr_robot
_motionControllerPtr->setPose(_previousPose);
}
//!< Robot tf
ros::Time timeNow = ros::Time::now();
tf::Vector3 translation(_previousPose.x, _previousPose.y, 0);
tf::Quaternion rotation;
rotation.setRPY(0, 0, _previousPose.theta);

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 @@ -442,25 +464,7 @@ namespace stdr_robot
_previousPose.theta);
odom.twist.twist = _motionControllerPtr->getVelocity();

_odomPublisher.publish(odom);

//!< 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::Transform robotToSensor(rot, trans);

_tfBroadcaster.sendTransform(
tf::StampedTransform(
robotToSensor,
ros::Time::now(),
getName(),
_sensors[i]->getFrameId()));
}
_odomPublisher.publish(odom);
Copy link
Member

Choose a reason for hiding this comment

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

In this function the same time can be used when publishing odometry and the pose tf, instead of ros::Time::now() twice, in order to avoid the issue that you mentioned in your ROS Answer.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

true. In fact, using the same timestamp also for the sensors made things better (though using static tf is much much better anyway and the reasonable thing to do)

}

/**
Expand Down