Hello ROS community,
I am trying to upgrade from Ubuntu 16.04 + ROS kinetic + Gazebo 7 to Ubuntu 18.04 + Ros Melodic + Gazebo 9.
I have a ROSLaunch file that runs gazebo with an environment and a node that publishes the ground truth position of the urdf robot. However, when running the node after the upgrade I get the error:
[ERROR] [1549876249.264445531, 7.551000000]: "odom" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1549876249.795771873, 8.077000000]: Lookup would require extrapolation at time 7.555000000, but only time 7.051000000 is in the buffer, when looking up transform from frame [camera_link] to frame [odom]
The transformation tree looks like this:
while in the previous version looked like this:
The launch file is:
<launch>
<!-- Launch the empty world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="worlds/empty.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- Spawn the apartment into Gazebo -->
<arg name="environment" default="test_apartment_2" />
<node name="spawn_sdf" pkg="gazebo_ros" type="spawn_model"
args="-sdf -file $(find lucrezio_simulation_environments)/models/$(arg environment)/model.sdf -model $(arg environment)" />
<!-- Upload object locations and spawn them into Gazebo-->
<include file="$(find lucrezio_simulation_environments)/launch/upload_object_locations.launch">
<arg name="environment" value="$(arg environment)" />
</include>
<node name="spawn_objects" pkg="lucrezio_simulation_environments" type="spawn_object.py" args="all"/>
<!-- push robot_description to factory and spawn robot in gazebo -->
<arg name="model" default="$(find lucrezio_simulation_environments)/urdf/lucrezio_with_logical.urdf.xacro"/>
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />
<node pkg="tf" type="static_transform_publisher" name="camera_link_broadcaster" args="0 0 0 -0.5 0.5 -0.5 0.5 /camera_link /camera_depth_optical_frame 20" />
<node pkg="tf" type="static_transform_publisher" name="base_footprint_broadcaster" args="0 0 0.1 0.0 0.0 0.0 1.0 /base_footprint /base_link 20" />
<!-- start state publishers -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
<!-- <param name="publish_frequency" type="double" value="100.0" />
</node>
-->
<!-- Run the controller manager -->
<rosparam command="load" file="$(find lucrezio_simulation_environments)/config/diffdrive.yaml" ns="lucrezio" />
<node name="lucrezio_controller_spawner" pkg="controller_manager" type="spawner" args="lucrezio --shutdown-timeout 3"/>
</launch>
and the node:
#include <iostream>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <gazebo_msgs/LinkStates.h>
#include <Eigen/Geometry>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <lucrezio_simulation_environments/LogicalImage.h>
class PoseBroadcaster{
public:
PoseBroadcaster(ros::NodeHandle nh_):
_nh(nh_),
_logical_image_sub(_nh,"/gazebo/logical_camera_image",1),
_scan_sub(_nh,"/scan",1),
_synchronizer(FilterSyncPolicy(1000),_logical_image_sub,_scan_sub){
_synchronizer.registerCallback(boost::bind(&PoseBroadcaster::filterCallback, this, _1, _2));
}
void filterCallback(const lucrezio_simulation_environments::LogicalImage::ConstPtr &logical_image_msg,
const sensor_msgs::LaserScan::ConstPtr &laser_msg){
ros::Time laser_stamp = laser_msg->header.stamp;
tf::StampedTransform camera_odom_tf;
try{
_listener.waitForTransform("/odom",
"/camera_link",
laser_stamp,
ros::Duration(0.5));
_listener.lookupTransform("/odom",
"/camera_link",
laser_stamp,
camera_odom_tf);
} catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
Eigen::Isometry3f camera_odom_transform = tfTransform2eigen(camera_odom_tf);
ros::Time image_stamp = logical_image_msg->header.stamp;
Eigen::Isometry3f camera_map_transform = poseMsg2eigen(logical_image_msg->pose);
tf::Transform odom_map_tf = eigen2tfTransform(camera_map_transform*camera_odom_transform.inverse());
_br.sendTransform(tf::StampedTransform(odom_map_tf, image_stamp, "/map", "/odom"));
// ROS_INFO("Laser msg stamp: %f", laser_stamp.toSec());
// ROS_INFO("Logical image stamp: %f", image_stamp.toSec());
// ROS_INFO("...");
std::cerr << ".";
}
Eigen::Isometry3f tfTransform2eigen(const tf::Transform& p){
Eigen::Isometry3f iso = Eigen::Isometry3f::Identity();
iso.translation().x()=p.getOrigin().x();
iso.translation().y()=p.getOrigin().y();
iso.translation().z()=p.getOrigin().z();
Eigen::Quaternionf q;
tf::Quaternion tq = p.getRotation();
q.x()= tq.x();
q.y()= tq.y();
q.z()= tq.z();
q.w()= tq.w();
iso.linear()=q.toRotationMatrix();
return iso;
}
Eigen::Isometry3f poseMsg2eigen(const geometry_msgs::Pose &p){
Eigen::Isometry3f iso = Eigen::Isometry3f::Identity();
iso.translation().x()=p.position.x;
iso.translation().y()=p.position.y;
iso.translation().z()=p.position.z;
Eigen::Quaternionf q;
q.x()=p.orientation.x;
q.y()=p.orientation.y;
q.z()=p.orientation.z;
q.w()=p.orientation.w;
iso.linear()=q.toRotationMatrix();
return iso;
}
tf::Transform eigen2tfTransform(const Eigen::Isometry3f& T){
Eigen::Quaternionf q(T.linear());
Eigen::Vector3f t=T.translation();
tf::Transform tft;
tft.setOrigin(tf::Vector3(t.x(), t.y(), t.z()));
tft.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w()));
return tft;
}
protected:
ros::NodeHandle _nh;
tf::TransformListener _listener;
tf::TransformBroadcaster _br;
Eigen::Isometry3f _camera_transform;
message_filters::Subscriber<lucrezio_simulation_environments::LogicalImage> _logical_image_sub;
message_filters::Subscriber<sensor_msgs::LaserScan> _scan_sub;
typedef message_filters::sync_policies::ApproximateTime<lucrezio_simulation_environments::LogicalImage,
sensor_msgs::LaserScan> FilterSyncPolicy;
message_filters::Synchronizer<FilterSyncPolicy> _synchronizer;
};
int main(int argc, char **argv){
ros::init(argc, argv, "pose_broadcaster_node");
ros::NodeHandle nh;
PoseBroadcaster dummy(nh);
ros::spin();
return 0;
}
Any help is very appreciated, Thanks!
Originally posted by JoseJaramillo on ROS Answers with karma: 16 on 2019-02-11
Post score: 0