octomap_server error: "Could not generate key for origin"
I have successfully run octomap_server to create a 3D grid, and visualize it in rviz. I have only been able to do this with octomap's frame_id corresponding to a static tf frame.
When I tried including my custom tf broadcaster, (which generates a transform between the robot and the world from INS position and orientation data), octomap_server continually generates the error: "Could not generate key for origin". I believe this is due to a mismatch in camera depth data, and tf frame time stamps, but I have not been able to find any documentation on this error.
Has anyone else dealt with this error? Or can you see a potential problem in my tf broadcaster code?
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
tf::Quaternion ins_quat(0, 0, 0, 1); //Initial values for INS transform frame from world
tf::Vector3 ins_vect(0, 0, 0);
void insCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
static tf::TransformBroadcaster broadcaster;
std::cout << "instide insCallback\n";
ins_quat = tf::Quaternion( //Collect the position data
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w
);
ins_vect = tf::Vector3(
msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z
);
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(
ins_quat, ins_vect
),
ros::Time::now(),
"world", "ins_link"
)
);
broadcaster.sendTransform( //TODO Should make this a static tf in the future
tf::StampedTransform(
tf::Transform(
tf::Quaternion(0, 0, -0.707, 0.707), tf::Vector3(0.165, 0.05, 0.345) //static orientation
),
ros::Time::now(),
"ins_link", "camera_link"
)
);
}
int main(int argc, char** argv){
ros::init(argc, argv, "tf_broadcaster");
ros::NodeHandle n;
ros::Subscriber ins_sub = n.subscribe("ins", 1, insCallback);
ros::spin();
}