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 1 commit
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 static_broadcaster;
Copy link
Member

Choose a reason for hiding this comment

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

Prepend underscore to comply with the coding styleguide (I know it's ugly...).

Copy link
Contributor Author

Choose a reason for hiding this comment

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

right


//!< 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
51 changes: 29 additions & 22 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,13 @@ namespace stdr_robot
_moveRobotService = n.advertiseService(
getName() + "/replace", &Robot::moveRobotCallback, this);

//we should not start the timer, until we hame a motion controller
//allow changing odometry rate, as the default 10Hz can be too slow in some cases
double odometry_rate;
n.param(getName() + "/odometry_rate", odometry_rate, 10.0);

//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(1.0/odometry_rate), &Robot::publishTransforms, this, false, false);
Copy link
Member

@czalidis czalidis Nov 13, 2017 •
edited
Loading

Choose a reason for hiding this comment

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

Could we put the default to 100Hz and not use the parameter? If you use the parameter you would need to check for division by zero. I also don't know what would be the computational impact if we increase to 100Hz though...

Copy link
Contributor Author

@corot corot Nov 13, 2017 •
edited
Loading

Choose a reason for hiding this comment

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

Impact is small, I get similar same CPU usage (~85% vs. ~90% in my machine). What is normal as the function is quite light. But 50 Hz would be equally fine.

}

/**
Expand Down Expand Up @@ -172,6 +176,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 static_transformStamped;

static_transformStamped.header.stamp = ros::Time::now();
static_transformStamped.header.frame_id = getName();
static_transformStamped.child_frame_id = _sensors[i]->getFrameId();
static_transformStamped.transform.translation.x = sensorPose.x;
static_transformStamped.transform.translation.y = sensorPose.y;
static_transformStamped.transform.translation.z = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, sensorPose.theta);
static_transformStamped.transform.rotation.x = quat.x();
static_transformStamped.transform.rotation.y = quat.y();
static_transformStamped.transform.rotation.z = quat.z();
static_transformStamped.transform.rotation.w = quat.w();
static_broadcaster.sendTransform(static_transformStamped);
}

_tfTimer.start();
}

Expand Down Expand Up @@ -407,7 +432,7 @@ namespace stdr_robot
}

/**
@brief Publishes the tf transforms every with 10Hz
@brief Publishes the tf transforms every with 10Hz (default)
@return void
**/
void Robot::publishTransforms(const ros::TimerEvent&)
Expand Down Expand Up @@ -442,25 +467,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