Update ROS 2 time_callback acccording to ROS 1

[horverno]

duro_gps_driver/src/duro.cpp

Lines 296 to 324 in 9d166b4

	void time_callback(u16 sender_id, u8 len, u8 msg[], void *context)
	{
	msg_gps_time_t *time_gps = (msg_gps_time_t *)msg;
	sensor_msgs::TimeReference time_msg;
	time_msg.header.frame_id = "ros_time";
	//time_msg.header.stamp.nsec = ros::Time::now().nsec / 1000;
	//time_msg.header.stamp.sec = ros::Time::now().sec;
	time_msg.header.stamp = ros::Time::now();
	//rounded msec + residual nsec -> truncated sec + remainder nsec
	long long int towtemp = time_gps->tow % 1000;
	long long int ttemp = (towtemp * 1000000 + time_gps->ns_residual) % 1000000000;
	time_msg.time_ref.nsec = ttemp;
	time_msg.time_ref.sec = time_gps->tow / 1000 + time_gps->wn * 604800 + 315964782;
	time_msg.source = "gps_duro";
	std_msgs::Float64 diff_msg;
	ros::Duration diff = time_msg.time_ref - time_msg.header.stamp;
	diff_msg.data = diff.toSec();
	//diff_msg.data = time_gps->tow;
	std_msgs::String gps_str_msg;
	gps_str_msg.data = std::to_string(time_gps->wn) + " " + std::to_string(time_gps->tow) + " " + std::to_string(time_gps->ns_residual);
	time_gps_str_pub.publish(gps_str_msg);
	time_ref_pub.publish(time_msg);
	time_diff_pub.publish(diff_msg);
	}

duro_gps_driver/src/duro_node.cpp

Lines 419 to 433 in 336fd80

	void time_callback(u16 sender_id, u8 len, u8 msg[], void *context)
	{
	msg_gps_time_t *time_gps = (msg_gps_time_t *)msg;
	time_ref_msg.header.frame_id = "ros_time_frame";
	time_ref_msg.header.stamp = node->now();
	//rounded msec + residual nsec -> truncated sec + remainder nsec
	long long int towtemp = time_gps->tow % 1000;
	long long int ttemp = (towtemp * 1000000 + time_gps->ns_residual) % 1000000000;
	time_ref_msg.time_ref.nanosec = ttemp;
	time_ref_msg.time_ref.sec = time_gps->tow / 1000;
	time_ref_msg.source = "gps_duro";
	time_ref_pub->publish(time_ref_msg);
	}