77 rclcpp::NodeOptions node_options;
78 node_options.allow_undeclared_parameters(
true);
79 node_options.automatically_declare_parameters_from_overrides(
true);
85 if (m_node ==
nullptr) {
90 rclcpp::Parameter simTime(
"use_sim_time", rclcpp::ParameterValue(
true ) );
91 m_node->set_parameter( simTime );
92 const std::string m_tf_topic =
"/tf";
93 m_publisher_tf = m_node->create_publisher<tf2_msgs::msg::TFMessage>(m_tf_topic, 10);
95 m_ros2Publisher_odometry = m_node->create_publisher<nav_msgs::msg::Odometry>(
m_topic_name, 10);
108 if (m_odometry2D_interface!=
nullptr && m_ros2Publisher_odometry && m_publisher_tf) {
110 double synchronized_timestamp = 0;
111 m_odometry2D_interface->
getOdometry(odometryData, &synchronized_timestamp);
113 if (std::isnan(synchronized_timestamp) ==
false)
115 m_timeStamp.
update(synchronized_timestamp);
122 nav_msgs::msg::Odometry odometryMsg;
126 odometryMsg.pose.pose.position.x = odometryData.
odom_x;
127 odometryMsg.pose.pose.position.y = odometryData.
odom_y;
128 odometryMsg.pose.pose.position.z = 0.0;
129 geometry_msgs::msg::Quaternion odom_quat;
131 double cosYaw = cos(halfYaw);
132 double sinYaw = sin(halfYaw);
135 odom_quat.z = sinYaw;
136 odom_quat.w = cosYaw;
137 odometryMsg.pose.pose.orientation = odom_quat;
138 odometryMsg.twist.twist.linear.x = odometryData.
base_vel_x;
139 odometryMsg.twist.twist.linear.y = odometryData.
base_vel_y;
140 odometryMsg.twist.twist.linear.z = 0;
141 odometryMsg.twist.twist.angular.x = 0;
142 odometryMsg.twist.twist.angular.y = 0;
146 tf2_msgs::msg::TFMessage rosData;
148 geometry_msgs::msg::TransformStamped tsData;
152 tsData.transform.rotation.x = 0;
153 tsData.transform.rotation.y = 0;
154 tsData.transform.rotation.z = sinYaw;
155 tsData.transform.rotation.w = cosYaw;
156 tsData.transform.translation.x = odometryData.
odom_x;
157 tsData.transform.translation.y = odometryData.
odom_y;
158 tsData.transform.translation.z = 0;
160 odometryMsg.header.stamp.sec = int(m_timeStamp.
getTime());
161 odometryMsg.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.
getTime() -
int(m_timeStamp.
getTime())));
163 tsData.header.stamp.sec = int(m_timeStamp.
getTime());
164 tsData.header.stamp.nanosec = int(1000000000UL * (m_timeStamp.
getTime() -
int(m_timeStamp.
getTime())));
166 if (rosData.transforms.size() == 0)
168 rosData.transforms.push_back(tsData);
170 else if (rosData.transforms.size() == 1)
172 rosData.transforms[0] = tsData;
179 if(m_ros2Publisher_odometry->get_subscription_count() > 0)
180 m_ros2Publisher_odometry->publish(odometryMsg);
182 m_publisher_tf->publish(rosData);
virtual yarp::dev::ReturnValue getOdometry(yarp::dev::Nav2D::Odometry &odom, double *timestamp=nullptr)=0
Gets the odometry of the robot, including its velocity expressed in the world and in the local refere...