I looked through the [tf setup](http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF), but it only show updating tf for a fixed system at arbitrary 1Hz interval.
Anyways, I plan on using sensor information to navigate a map. So my tf tree will be something like this
/world-->/map-->/odom-->/base_link-->/gimbal-->/sensor
/word===/map???
I have a node that grabs the gimbal and odom information from the robot platform 60 times a second and publishes the information to their respective topics. Thus should I update broadcast tf information in the same node?
(Note the code is just a simple rough outline of what I'm trying to do. My actual code will probably be setup with classes so I can have tracking variables).
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot");
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
MyRobotClass robot; //just some class for my robot interface
nav_msgs::Odometry odometry;
my_msg::Gimbal gimbal
// simple gimbal message with header, yaw, roll, and pitch
tf::TransformBoardcaster odom_broadcaster;
tf::TransformBoardcaster gimbal_broadcaster;
ros::Publisher gimbal_publisher;
ros::Publisher global_position_publisher;
gimbal_publisher = nh.advertise("gimbal", 10);
odometry_publisher = nh.advertise("odometry",10);
ros::Rate(60)
while(ros::ok())
{
time = ros::Time::now();
// update and publish odometry
odometry.header.frame_id = "/world";
odometry.header.stamp = current_time;
odometry.pose.pose.position.x = robot.position.x;
odometry.pose.pose.position.y = robot.position.y;
odometry.pose.pose.position.z = robot.position.z;
odometry.pose.pose.orientation.w = robot.quaternion.q0;
odometry.pose.pose.orientation.x = robot.quaternion.q1;
odometry.pose.pose.orientation.y = robot.quaternion.q2;
odometry.pose.pose.orientation.z = robot.quaternion.q3;
odometry.twist.twist.angular.x = robot.velocity.wx;
odometry.twist.twist.angular.y = robot.velocity.wy;
odometry.twist.twist.angular.z = robot.velocity.wz;
odometry.twist.twist.linear.x = robot.velocity.vx;
odometry.twist.twist.linear.y = robot.velocity.vy;
odometry.twist.twist.linear.z = robot.velocity.vz;
odometry_publisher.publish(odometry);
// update and publish gimbal
gimbal.header.frame_id = "/gimbal";
gimbal.header.stamp= current_time;
gimbal.roll = robot.gimbal.roll;
gimbal.pitch = robot.gimbal.pitch;
gimbal.yaw = robot.gimbal.yaw;
gimbal_publisher.publish(gimbal);
// update tf of odom here????
tf::Vector3 vectorOdom(robot.position.x, robot.position.y, robot.position.z);
tf::Quaternion quatOdom(robot.quaternion.q1, robot.quaternion.q2, robot.quaternion.q3, robot.quaternion.q0);
tf::Transform transformOdom(quatOdom, vectorOdom);
odom_broadcaster.sendTransform(transformOdom, time, "/map", "/base_link")
// update tf of gimbal here?
tf::Vector3 vectorGimbal(0.0, 0.0, 0.0);
tf::Quaternion quatGimbal;
quatGimbal.setEuler(robot.gimbal.yaw, robot.gimbal.pitch, robot.gimbal.roll);
tf::Transform transformGimbal(quatGimbal, vectorGimbal);
gimbal_broadcaster.sendTransform(transformGimbal, time, "/base_link", "/gimbal")
}
retrun 0;
}
Then I have a node using the camera obtain position of object in reference to the camera.
void imageCb(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& cam_info)
{
geometry_msg::PoseStamped tagPose;
//code that generates tag pose in reference to the camera in POSE_TAG_DATA
tagPose.header.frame_id = "/camera";
tagPose.header.stamp = msg->header.stamp;
tagPose.header.seq = msg->header.seq;
tagPose.pose = POSE_TAG_DATA; //pose of the tag
pose_pub.publish(tagPose);
// Need to flip axis from camera frame to match frame of gimbal? (x, y, z) will map to (y, -z, x)
tf::Vector3 vectorTag(tagPose.pose.y, tagPose.pose.z * -1, tagPose.pose.x);
// For now I just need position, but maybe orientation in the future?
tf::Quaternion quatTag (0.0, 0.0, 0.0, 1.0);
tf::Transform transformTag(quatTag, vectorTag);
camera_boardcaster.sendTransform(transformTag, msg->header.stamp, "/gimbal", "/camera");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot");
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
ros::Publisher pose_pub
image_transport::ImageTransport it;
image_transport::CameraSubscriber image_sub;
tf::TransformBroadcaster camera_boardcaster;
tag_publisher = nh.advertise("tagPose", 10);
image_sub = it.subscribeCamera("image_raw", 1, &imageCb);
ros::spin();
return 0;
}
I guess my question is does the tf setup look correct?
Finally I need to write Node to subscribes "tagPose" to set as navigation goal. How would tf tree help me transform the tagPose pose in the "/camera" frame to the "/map" frame? What will the basic structure of the callbackfunction for subscribing to "tagPose" and listening to tf for "/camera" to "/map" look like?
Is this, [Using Stamped datatypes with tf::MessageFilter](http://wiki.ros.org/tf/Tutorials/Using%20Stamped%20datatypes%20with%20tf%3A%3AMessageFilter) the tutorial I should follow to setup using sensor and tf frames together with stamps?
My goal is to have robot travel to the tag, but processing tag pose has a large delay of around 20-100ms. Thus I need to make sure the tag pose is transform into the world coordinates based on odom and gimbal states at the time the camera image was taken (i.e. 20-100ms in the past). My understanding is that tf should handle this with the correct setup?
↧