Closed dzywater closed 2 years ago
(1)in rotors_gazebo_plugins\package.xml use
instead of
(2)in rotors_gazebo_plugins\CMakeLists.txt use tf2_ros instead of tf
(3)in rotors_gazebo_plugins\src\gazebo_noisydepth_plugin.cpp remove #include <tf/tf.h>
(4) in rotors_gazebo_plugins\include\rotors_gazebo_plugins\gazebo_ros_interface_plugin.h use tf2_ros::TransformBroadcaster transformbroadcaster; instead of tf::Transform tf_; tf::TransformBroadcaster transformbroadcaster;
(5) rotors_gazebo_plugins\src\gazebo_ros_interface_plugin.cpp rewrite GzBroadcastTransformMsgCallback
void GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback( GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg) { geometry_msgs::TransformStamped msg; msg.header.stamp.sec = broadcast_transform_msg->header().stamp().sec(); msg.header.stamp.nsec = broadcast_transform_msg->header().stamp().nsec(); msg.header.frame_id = broadcast_transform_msg->parent_frame_id(); msg.child_frame_id = broadcast_transform_msg->child_frame_id();
msg.transform.rotation.x = broadcast_transform_msg->transform().rotation().x(); msg.transform.rotation.y = broadcast_transform_msg->transform().rotation().y(); msg.transform.rotation.z = broadcast_transform_msg->transform().rotation().z(); msg.transform.rotation.w = broadcast_transform_msg->transform().rotation().w();
msg.transform.translation.x = broadcast_transform_msg->transform().translation().x(); msg.transform.translation.y = broadcast_transform_msg->transform().translation().y(); msg.transform.translation.z = broadcast_transform_msg->transform().translation().z();
transformbroadcaster.sendTransform(msg); }
(1)in rotors_gazebo_plugins\package.xml use
instead of
(2)in rotors_gazebo_plugins\CMakeLists.txt use tf2_ros instead of tf
(3)in rotors_gazebo_plugins\src\gazebo_noisydepth_plugin.cpp remove #include <tf/tf.h>
(4) in rotors_gazebo_plugins\include\rotors_gazebo_plugins\gazebo_ros_interface_plugin.h use tf2_ros::TransformBroadcaster transformbroadcaster; instead of tf::Transform tf_; tf::TransformBroadcaster transformbroadcaster;
(5) rotors_gazebo_plugins\src\gazebo_ros_interface_plugin.cpp rewrite GzBroadcastTransformMsgCallback
void GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback( GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg) { geometry_msgs::TransformStamped msg; msg.header.stamp.sec = broadcast_transform_msg->header().stamp().sec(); msg.header.stamp.nsec = broadcast_transform_msg->header().stamp().nsec(); msg.header.frame_id = broadcast_transform_msg->parent_frame_id(); msg.child_frame_id = broadcast_transform_msg->child_frame_id();
msg.transform.rotation.x = broadcast_transform_msg->transform().rotation().x(); msg.transform.rotation.y = broadcast_transform_msg->transform().rotation().y(); msg.transform.rotation.z = broadcast_transform_msg->transform().rotation().z(); msg.transform.rotation.w = broadcast_transform_msg->transform().rotation().w();
msg.transform.translation.x = broadcast_transform_msg->transform().translation().x(); msg.transform.translation.y = broadcast_transform_msg->transform().translation().y(); msg.transform.translation.z = broadcast_transform_msg->transform().translation().z();
transformbroadcaster.sendTransform(msg); }