CustomOdometry publishing added.

AndreaFinazzi · May 29, 2019 · df64718 · df64718
1 parent 6d0530c
commit df64718
Show file tree

Hide file tree

Showing 6 changed files with 125 additions and 62 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -59,7 +59,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate messages in the 'msg' folder
 
 add_message_files(FILES
-  OdometryStamped.msg
+  CustomOdometry.msg
   floatStamped.msg
 )
 

diff --git a/README.txt b/README.txt
@@ -0,0 +1,37 @@
+Project team members:
+	-871569, Grazioli, Enrico
+	-868454, Finazzi, Andrea
+	-865784, Colombo, Marco
+
+The archive contains the ros package directory "odometry" structured as follows:
+	-CMakeList.txt: automatically created by ros. It contains all the configurations
+			used by ros to build the executables of the package such as dependencies,
+			packege files names etc.
+	-package.xml: another ros generated file with similar porpouse to the CMakeList file.
+	-frames.pdf: scheme of the tf tree.
+	-[dir]msg: contains .msg files used to describe custom messages.
+		-CustomOdometry.msg: message type published on "custom_odometry" topic.
+				     It contains Odometry.msg information on computed odometry
+				     and a strig specifing the odometry model adopted.
+		-floatStamped.msg: message type used to read messages generated by the .bag
+				   files provided.
+	-[dir]src: contains the .cpp source code files
+		-odometry_node: it's the only source file of the packege and contains the
+				odometry computation function and all the subscribig/publishing
+				logic.
+	-[dir]cfg:
+
+
+Configuration.....finaz
+
+
+Custom messages:
+	-odometry/floatStamped:
+		std_msgs/Header header
+		std_msgs/float64 data
+
+	-odometry/CustomOdometry
+		std_msgs/String odometryModel
+		nav_msgs/Odometry odometry
+
+
diff --git a/frames.pdf b/frames.pdf
diff --git a/msg/CustomOdometry.msg b/msg/CustomOdometry.msg
@@ -0,0 +1,2 @@
+std_msgs/String odometryMode
+nav_msgs/Odometry odometry
diff --git a/msg/OdometryStamped.msg b/msg/OdometryStamped.msg
diff --git a/src/odometry_node.cpp b/src/odometry_node.cpp
@@ -1,5 +1,3 @@
-#include<math.h>
-
 #include "ros/ros.h"
 #include "ros/duration.h"
 
@@ -16,14 +14,17 @@
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
 
-//#include "custom_messages/floatStamped.h"
 #include "odometry/floatStamped.h"
+#include "odometry/CustomOdometry.h"
+
 
+// Numeric constants
 #define BASELINE 1.30
 #define CAR_LENGTH 1.765
 #define STEERING_FACTOR 18
 #define PI 3.14159265
 
+// Default constants
 #define DEFAULT_INIT_POSITION_X 0.0
 #define DEFAULT_INIT_POSITION_Y 0.0
 #define DEFAULT_INIT_POSITION_THETA 0.0
@@ -34,18 +35,22 @@
 #define DEFAULT_INIT_VELOCITY_RIGHT 0.0
 #define DEFAULT_INIT_STEER 0.0
 
-
+// Configuration constants
 #define DIFFERENTIAL_MODEL_MODE true
-#define ACKERMAN_MODEL_MODE false
+#define ACKERMANN_MODEL_MODE false
 
+// Topic/Frame names definition
 #define SPEED_L_TOPIC "speedL_stamped"
 #define SPEED_R_TOPIC "speedR_stamped"
 #define STEER_TOPIC "steer_stamped"
 
+#define CUSTOM_ODOMETRY_TOPIC_ID "custom_odometry"
 #define ODOMETRY_TOPIC_ID "odometry"
 #define ODOMETRY_FRAME_ID "odometry"
 #define CHILD_FRAME_ID "base_link"
 
+
+// Support data structures
 typedef struct last_odometry_data {
     double x;
     double y;
@@ -76,7 +81,7 @@ void resetDefaultPosition() {
 }
 
 void configChangeCallback(odometry::parametersConfig& config, uint32_t level) {
-    ROS_INFO("Reconfigure Request:\nodometry_model_mode: %s\nodometry_reset_default: %s\nodometry_set_position: %s\nodometry_x_position: %f\nodometry_y_position: %f",
+    ROS_INFO("Reconfigure Request:\n\todometry_model_mode: %s\n\todometry_reset_default: %s\n\todometry_set_position: %s\n\todometry_x_position: %f\n\todometry_y_position: %f\n",
           config.odometry_model_mode?"True":"False",
           config.odometry_reset_default?"True":"False",
           config.odometry_set_position?"True":"False",
@@ -92,75 +97,81 @@ void configChangeCallback(odometry::parametersConfig& config, uint32_t level) {
         resetDefaultPosition();
 }
 
-void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R, OdometryData& new_odometry_data){
+void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){
 
 
-    double linear_velocity = (speed_R + speed_L) / 2;
-    double angular_velocity = (speed_R - speed_L) / BASELINE;
-    new_odometry_data.angular_velocity = angular_velocity;
+    double linear_velocity = (last_odometry_data.velocity_R + last_odometry_data.velocity_L) / 2;
+    double angular_velocity = last_odometry_data.angular_velocity;
 
     double delta_theta = angular_velocity * delta_time;
 
     new_odometry_data.theta = last_odometry_data.theta + delta_theta;
 
-    new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta);
-    new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta);
-
-
 
+
     if (new_odometry_data.theta > 2*PI) {
         new_odometry_data.theta -= 2*PI;
     } else if (new_odometry_data.theta < 0) {
         new_odometry_data.theta += 2*PI;
     }
 
-    if (angular_velocity > 0.0001)  //exact integration assuming velocities constamìnt in the time lapse
-    {
-        new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta));
-        new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta));
+    new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2);
+    new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2);
+
+
+    double new_linear_velocity = (speed_R + speed_L) / 2;
+
+    new_odometry_data.angular_velocity = (speed_R - speed_L) / BASELINE;
+
+    new_odometry_data.linear_velocity_x = new_linear_velocity * cos(new_odometry_data.theta);
+    new_odometry_data.linear_velocity_y = new_linear_velocity * sin(new_odometry_data.theta);
+
+    new_odometry_data.steer = steer;
+    new_odometry_data.velocity_L = speed_L;
+    new_odometry_data.velocity_R = speed_R;
+
 
-    } else {
-        new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2);
-        new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2);
-    }
 }
 
 
-void ackermanDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){
-    
+void ackermannDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){
+
 
-    double linear_velocity = (speed_R + speed_L) / 2;
-    double angular_velocity = linear_velocity * tan((steer / STEERING_FACTOR) * (PI / 180)) / CAR_LENGTH;
-    new_odometry_data.angular_velocity = angular_velocity;
+    double linear_velocity = (last_odometry_data.velocity_R + last_odometry_data.velocity_L) / 2;
+    double angular_velocity = last_odometry_data.angular_velocity;
 
     double delta_theta = angular_velocity * delta_time;
 
-
     new_odometry_data.theta = last_odometry_data.theta + delta_theta;
-
-    new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta);
-    new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta);
 
 
     if (new_odometry_data.theta > 2*PI) {
         new_odometry_data.theta -= 2*PI;
     } else if (new_odometry_data.theta < 0) {
         new_odometry_data.theta += 2*PI;
     }
+
+    new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2);
+    new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2);
+
+    double new_linear_velocity = (speed_R + speed_L) / 2;
+
+    new_odometry_data.angular_velocity = new_linear_velocity * tan((steer / STEERING_FACTOR) * (PI / 180)) / CAR_LENGTH;
+
+    new_odometry_data.linear_velocity_x = new_linear_velocity * cos(new_odometry_data.theta);
+    new_odometry_data.linear_velocity_y = new_linear_velocity * sin(new_odometry_data.theta);
+
+    new_odometry_data.steer = steer;
+    new_odometry_data.velocity_L = speed_L;
+    new_odometry_data.velocity_R = speed_R;
+
 
-    if (angular_velocity > 0.0001)  //exact integration assuming velocities constamìnt in the time lapse
-    {
-        new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta));
-        new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta));
 
-    } else {
-        new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2);
-        new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2);
-    }
 }
 
 nav_msgs::Odometry populateOdometry() {
 
+
     nav_msgs::Odometry odometry;
     odometry.header.stamp = ros::Time::now();
     odometry.header.frame_id = ODOMETRY_FRAME_ID;
@@ -196,40 +207,53 @@ geometry_msgs::TransformStamped populateTransform() {
     return odometry_transform;
 }
 
+odometry::CustomOdometry populateCustomOdometry(nav_msgs::Odometry odometry, string odometryModel) {
+
+    odometry::CustomOdometry customOdometry;
+    customOdometry.odometry = odometry;
+
+    customOdometry.odometryMode.data = odometryModel;
+
+    return customOdometry;
+
+}
+
 void subCallback(const odometry::floatStamped::ConstPtr& left,
                  const odometry::floatStamped::ConstPtr& right, 
                  const odometry::floatStamped::ConstPtr& steer,
                  tf::TransformBroadcaster& broadcaster,
-                 ros::Publisher& publisher){
+                 ros::Publisher& publisherOdometry,
+                 ros::Publisher& publisherCustomOdometry){
 
+    string odometryModel;
     double time = (left->header.stamp.toSec() + right->header.stamp.toSec() + steer->header.stamp.toSec()) / 3;
     double delta_time = time - last_msg_time;
     last_msg_time = time;
 
     OdometryData new_odometry_data;
     if (last_config.odometry_model_mode == DIFFERENTIAL_MODEL_MODE) {
-        differenrialDriveOdometry(delta_time, left->data, right->data, new_odometry_data);
-    } else if (last_config.odometry_model_mode == ACKERMAN_MODEL_MODE) {
-        ackermanDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data);
+        differenrialDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data);
+        odometryModel = "Differential Drive";
+    } else if (last_config.odometry_model_mode == ACKERMANN_MODEL_MODE) {
+        ackermannDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data);
+        odometryModel = "Ackermann Drive";
     } else {
-        ROS_INFO("!!!!!!!!!!! CRITICAL CONFIG ERROR - ODOMETRY MODEL NOT SET!!!!!!!!!!!");
+        ROS_INFO("!!!!!!!!!!! CRITICAL CONFIG ERROR - ODOMETRY MODEL NOT SET!!!!!!!!!!!\n\n");
     }
-
-
-    double deltax = new_odometry_data.x - last_odometry_data.x;
-    double deltay = new_odometry_data.y - last_odometry_data.y;
-    double speed = (right->data + left->data) / 2;
 
     last_odometry_data = new_odometry_data;
 
-    ROS_INFO("[\nX COORDINATE: %f\nY COORDINATE: %f\nORIENTATION: %f\ndelta_time: %f\nSPEED: %f\nDELTA: %f   %f", last_odometry_data.x, last_odometry_data.y, last_odometry_data.theta * 360/ (2*PI), delta_time, speed, deltax, deltay);
 
     // publishing and broadcasting
     nav_msgs::Odometry odometry = populateOdometry();
-    publisher.publish(odometry);
+    publisherOdometry.publish(odometry);
 
     geometry_msgs::TransformStamped transform = populateTransform();
     broadcaster.sendTransform(transform);
+
+    odometry::CustomOdometry customOdometry = populateCustomOdometry(odometry, odometryModel);
+    publisherCustomOdometry.publish(customOdometry);
+
 }
 
 
@@ -249,30 +273,34 @@ int main(int argc, char *argv[])
     last_odometry_data.velocity_R = DEFAULT_INIT_VELOCITY_RIGHT;
     last_odometry_data.steer = DEFAULT_INIT_STEER;
     last_msg_time = ros::Time::now().toSec();
+    ROS_INFO("SUPPORT DATA STRUCTURES INITIALIZED\n");
 
     // Config server init
     dynamic_reconfigure::Server<odometry::parametersConfig> config_server;
     config_server.setCallback(boost::bind(&configChangeCallback, _1, _2));
 
     // ### BEGIN ### output data publisher and broadcaster
     tf::TransformBroadcaster odometry_broadcaster;
-    ros::Publisher odometry_publisher = nh.advertise<nav_msgs::Odometry>("odometry", 50);
+    ros::Publisher odometry_publisher = nh.advertise<nav_msgs::Odometry>(ODOMETRY_TOPIC_ID, 50);
+    ros::Publisher custom_odometry_publisher = nh.advertise<odometry::CustomOdometry>(CUSTOM_ODOMETRY_TOPIC_ID ,50);
+
+    ROS_INFO("NODE STARTED\n");
 
     // ### BEGIN ### wheels data subscriber and synchronizer
-    std::cout << "Node started...\n";
     message_filters::Subscriber<odometry::floatStamped> left_speed_sub(nh, SPEED_L_TOPIC, 1);
     message_filters::Subscriber<odometry::floatStamped> right_speed_sub(nh, SPEED_R_TOPIC, 1);
     message_filters::Subscriber<odometry::floatStamped> steer_sub(nh, STEER_TOPIC, 1);
 
     typedef message_filters::sync_policies::ApproximateTime<odometry::floatStamped, odometry::floatStamped, odometry::floatStamped> syncPolicy;
 
-    std::cout << "Subscribers built...\n";
+    ROS_INFO("SUBSCRIBER BUILT\n");
     message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), left_speed_sub, right_speed_sub, steer_sub);
-    std::cout << "Synchronizer started...\n";
-    sync.registerCallback(boost::bind(&subCallback, _1, _2, _3, odometry_broadcaster, odometry_publisher));
-    std::cout << "Topics synchronized...\n";
+    ROS_INFO("SYNCHRONIZER STARTED\n");
+    sync.registerCallback(boost::bind(&subCallback, _1, _2, _3, odometry_broadcaster, odometry_publisher, custom_odometry_publisher));
+    ROS_INFO("TOPICS SYNCHRONIZED\n");
     // ### END ###
 
+    ROS_INFO("NODE RUNNING.....\n");
     ros::spin();
     return 0;
 }
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,2 @@
		std_msgs/String odometryMode
		nav_msgs/Odometry odometry