After migration from Kinetic to Melodic my script doesn't work [closed]

edit

Hi everyone. I'm new in ROS, sorry for my ignorance. I wrote a script in Kinetic that implements a trajectory tracking (track: y=0) for my unicicle in Gazebo. My script subscribe /odom and convert in rpy angles. Then, publishes /cmd_vel with a specific angular rate. In Kinetic, my robot follows the line correctly. After migration in Melodic, unicicle doesn't follow the line and goes round.

EDIT: I've implemented the suggestions. This is my code in C++.

#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose2D.h"
#include "nav_msgs/Odometry.h"
#include <tf/transform_datatypes.h>


/*Orientation*/
geometry_msgs::Pose2D pose2d; //x, y, theta

float my_sinc(float theta);
geometry_msgs::Twist control_law(geometry_msgs::Pose2D  pose2d, float lin_vel, float k);
void print_info(geometry_msgs::Pose2D pose2d, geometry_msgs::Twist control);

void pose_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
    //conversione quaternioni -> RPY
    tf::Quaternion q(
        msg->pose.pose.orientation.x,
        msg->pose.pose.orientation.y,
        msg->pose.pose.orientation.z,
        msg->pose.pose.orientation.w);
    tf::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    //update pose2d
    pose2d.x = msg->pose.pose.position.x;
    pose2d.y = msg->pose.pose.position.y;
    pose2d.theta = yaw;
}

int main(int argc,  char **argv)
{
    //Node init
    ros::init(argc, argv, "feedback2");
    ros::NodeHandle node_obj;

    //Pub
    ros::Publisher pub_obj=node_obj.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
    //Sub
    ros::Subscriber subscriber_obj=node_obj.subscribe("/odom", 1000, pose_callback);
    ros::Rate loop_rate(100);

    float lin_vel=1;
    float K=2;

    while (ros::ok())
    {
        ros::spinOnce(); //receive odom
        geometry_msgs::Twist vel = control_law(pose2d, lin_vel, K);
        pub_obj.publish(vel);
        print_info(pose2d, vel);
        loop_rate.sleep();
    }

    return 0;
}

float my_sinc(float theta)
{
    if(theta ==0)
    {
        return 1;
    }

    return sin(theta)/theta;
}

geometry_msgs::Twist control_law(geometry_msgs::Pose2D  pose2d, float lin_vel, float k)
{
    geometry_msgs::Twist vel;

    //default 2D constraints
    vel.linear.y=0;
    vel.linear.z=0;
    vel.angular.x=0;
    vel.angular.y=0;

    vel.linear.x=lin_vel;
    vel.angular.z=-(pose2d.y)*(my_sinc(pose2d.theta))*(lin_vel) -k*(pose2d.theta);

    return vel;

}

void print_info(geometry_msgs::Pose2D pose2d, geometry_msgs::Twist control)
{
    ROS_WARN("Information:\n");
    ROS_INFO("Position: [%f, %f]", pose2d.x, pose2d.y);
    ROS_INFO("Theta: %f", pose2d.theta);
    ROS_INFO("Linear Velocity: %f, Angular velocity: %f", control.linear.x, control.angular.z);
}