Using visual landmark to set tf from world to odom
I am using ros indigo with tf for a robot navigation problem. I am new to using tf so I am a bit confused. My robot has a standard encoders/IMU setup and I am using ekf_localization_node to compute my odom frame. That all works great.
My problem is that I have a visual localization system based on aprilTags which occasionally provides a very accurate base_link position with respect to the world frame.
My first thought was to let the aprilTag detection set odom->world as if it were static in between sightings since the aprilTag is the only link between odom->world. The visual localization provides the robot's pose so I tied to transform the localization pose back to the odom frame and then use that to compute the world->odom transform. Then I just publish that transform until a new observation updates it.
Here is my code: (visualLoactionMsg has the robots pose with respect to world).
geometry_msgs::PoseStamped transformedPose;
geometry_msgs::PoseStamped observedPose;
observedPose.pose = visualLocationMsg.pose.pose;
observedPose.header.stamp = ros::Time(0);
observedPose.header.frame_id = name+"/base_link";
try{
listener.transformPose(name+"/odom", observedPose, transformedPose);
lastTransform.setOrigin(
tf::Vector3(
transformedPose.pose.position.x,
transformedPose.pose.position.y,
transformedPose.pose.position.z
));
lastTransform.setRotation(
tf::Quaternion(
transformedPose.pose.orientation.x,
transformedPose.pose.orientation.y,
transformedPose.pose.orientation.z,
transformedPose.pose.orientation.w)
);
lastTransform = lastTransform.inverse();
}
...
broadcaster.sendTransform(tf::StampedTransform(
lastTransform, ros::Time::now(),
"/world", name+"/odom")
);
This seems to transform the position correctly, but not the orientation.
This may be in a tutorial somewhere, but I have been struggling with it for a while and figured I would see if anyone has a straightforward approach or could direct me to a working example. any advise would be appreciated.
Thanks, Jonathan West