diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index d016a73..cd980f3 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -9,6 +9,7 @@ #include "nav_msgs/Odometry.h" #include "sensor_msgs/Imu.h" #include "sensor_msgs/JointState.h" +#include "std_srvs/Trigger.h" #include "leo_msgs/Imu.h" #include "leo_msgs/WheelOdom.h" @@ -16,6 +17,8 @@ #include "leo_msgs/SetImuCalibration.h" +constexpr double PI = 3.141592653; + static ros::Subscriber wheel_states_sub; static ros::Publisher joint_states_pub; static bool joint_states_advertised = false; @@ -28,6 +31,16 @@ static ros::Subscriber imu_sub; static ros::Publisher imu_pub; static bool imu_advertised = false; +static ros::Publisher odom_merged_pub; +static ros::Timer odom_merged_timer; +static geometry_msgs::Point odom_merged_position; +static double odom_merged_yaw; +static bool odom_merged_advertised = false; +static double velocity_linear_x = 0; +static double velocity_angular_z = 0; +static ros::ServiceClient odom_reset_client; +static ros::ServiceServer reset_odometry_service; + ros::ServiceServer set_imu_calibration_service; std::string robot_frame_id = "base_link"; @@ -37,6 +50,7 @@ std::vector wheel_joint_names = { "wheel_FL_joint", "wheel_RL_joint", "wheel_FR_joint", "wheel_RR_joint"}; std::vector wheel_odom_twist_covariance_diagonal = {0.0001, 0.0, 0.0, 0.0, 0.0, 0.001}; + std::vector imu_angular_velocity_covariance_diagonal = { 0.000001, 0.000001, 0.00001}; std::vector imu_linear_acceleration_covariance_diagonal = {0.001, 0.001, @@ -90,12 +104,15 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) { wheel_odom.pose.pose.orientation.z = std::sin(msg->pose_yaw * 0.5F); wheel_odom.pose.pose.orientation.w = std::cos(msg->pose_yaw * 0.5F); + velocity_linear_x = msg->velocity_lin; + for (int i = 0; i < 6; i++) wheel_odom.twist.covariance[i * 7] = wheel_odom_twist_covariance_diagonal[i]; wheel_odom_pub.publish(wheel_odom); } + void imu_callback(const leo_msgs::ImuPtr& msg) { sensor_msgs::Imu imu; imu.header.frame_id = imu_frame_id; @@ -107,6 +124,8 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { imu.linear_acceleration.y = msg->accel_y; imu.linear_acceleration.z = msg->accel_z; + velocity_angular_z = imu.angular_velocity.z; + for (int i = 0; i < 3; i++) { imu.angular_velocity_covariance[i * 4] = imu_angular_velocity_covariance_diagonal[i]; @@ -117,6 +136,35 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { imu_pub.publish(imu); } +void merge_odometry_callback(const ros::TimerEvent& events) { + nav_msgs::Odometry merged_odom; + merged_odom.header.frame_id = odom_frame_id; + merged_odom.child_frame_id = robot_frame_id; + merged_odom.header.stamp = ros::Time::now(); + merged_odom.twist.twist.linear.x = velocity_linear_x; + merged_odom.twist.twist.angular.z = velocity_angular_z; + + const double move_x = velocity_linear_x * std::cos(odom_merged_yaw); + const double move_y = velocity_linear_x * std::sin(odom_merged_yaw); + + odom_merged_position.x += move_x * 0.01; + odom_merged_position.y += move_y * 0.01; + + odom_merged_yaw += velocity_angular_z * 0.01; + + if (odom_merged_yaw > 2.0 * PI) + odom_merged_yaw -= 2.0 * PI; + else if (odom_merged_yaw < 0.0) + odom_merged_yaw += 2.0 * PI; + + merged_odom.pose.pose.position.x = odom_merged_position.x; + merged_odom.pose.pose.position.y = odom_merged_position.y; + merged_odom.pose.pose.orientation.z = std::sin(odom_merged_yaw * 0.5); + merged_odom.pose.pose.orientation.w = std::cos(odom_merged_yaw * 0.5); + + odom_merged_pub.publish(merged_odom); +} + void load_yaml_bias() { YAML::Node node; try { @@ -173,6 +221,20 @@ bool set_imu_calibration_callback(leo_msgs::SetImuCalibrationRequest& req, return true; } +bool reset_odometry_callback(std_srvs::TriggerRequest& req, + std_srvs::TriggerResponse& res) { + odom_merged_position.x = 0.0; + odom_merged_position.y = 0.0; + odom_merged_yaw = 0.0; + if (odom_reset_client.call(req, res)) { + res.success = true; + return true; + } else { + res.success = false; + return false; + } +} + int main(int argc, char** argv) { ros::init(argc, argv, "firmware_message_converter"); @@ -190,10 +252,16 @@ int main(int argc, char** argv) { ros::names::resolve("firmware/wheel_states"); const std::string wheel_odom_topic = ros::names::resolve("firmware/wheel_odom"); + const std::string wheel_odom_mecanum_topic = + ros::names::resolve("firmware/wheel_odom_mecanum"); const std::string imu_topic = ros::names::resolve("firmware/imu"); set_imu_calibration_service = nh.advertiseService("set_imu_calibration", set_imu_calibration_callback); + odom_reset_client = + nh.serviceClient("firmware/reset_odometry"); + reset_odometry_service = + nh.advertiseService("reset_odometry", &reset_odometry_callback); ros::Rate rate(2); while (ros::ok()) { @@ -209,18 +277,25 @@ int main(int argc, char** argv) { if (wheel_odom_advertised && wheel_odom_sub.getNumPublishers() == 0) { ROS_INFO( "firmware/wheel_odom topic no longer has any publishers. " - "Shutting down wheel_odom_with_covariance publisher."); + "Shutting down wheel_odom_with_covariance and odometry_merged " + "publishers."); wheel_odom_sub.shutdown(); wheel_odom_pub.shutdown(); + odom_merged_pub.shutdown(); + odom_merged_timer.stop(); wheel_odom_advertised = false; + odom_merged_advertised = false; } if (imu_advertised && imu_sub.getNumPublishers() == 0) { ROS_INFO( "firmware/imu topic no longer has any publishers. " - "Shutting down imu/data_raw publisher."); + "Shutting down imu/data_raw and odometry_merged publishers."); imu_sub.shutdown(); imu_pub.shutdown(); + odom_merged_pub.shutdown(); + odom_merged_timer.stop(); imu_advertised = false; + odom_merged_advertised = false; } rate.sleep(); @@ -258,6 +333,16 @@ int main(int argc, char** argv) { imu_sub = nh.subscribe("firmware/imu", 5, imu_callback); imu_advertised = true; } + if (!odom_merged_advertised && imu_advertised && wheel_odom_advertised) { + ROS_INFO( + "Both firmware/imu and firmware/wheel_odom topics are advertised. " + "Advertising odometry_merged topic."); + odom_merged_pub = + nh.advertise("odometry_merged", 10); + odom_merged_timer = + nh.createTimer(ros::Duration(0.01), merge_odometry_callback); + odom_merged_advertised = true; + } } rate.sleep();