rviz robot moving in place

asked 2019-10-21 07:25:41 -0600

kallivalli gravatar image

updated 2019-10-21 23:20:46 -0600

hello everyone ,

I'm trying to do gmapping ,i have two rotary encoders(left and right) while moving my robot moves in place in an anticlockwise direction in RVIZ checked for both encoders(L,R) odom spins on the same anti-clockwise direction

im getting almost same rpm on both side

here is my code

here is odom node:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>
#include <stdio.h>
#include <cmath>
#include <algorithm>
#include <robot_specs.h>


double rpm_act1 = 0.0;
double rpm_act2 = 0.0;
double rpm_req1 = 0.0;
double rpm_req2 = 0.0;
double gyro_x = 0.0;
double gyro_y = 0.0;
double gyro_z = 0.0;
double rpm_dt = 0.0;
double x_pos = 0.0;
double y_pos = 0.0;
double theta = 0.0;


ros::Time current_time;
ros::Time rpm_time(0.0);
ros::Time last_time(0.0);

void handle_rpm( const geometry_msgs::Vector3Stamped& rpm) {
  rpm_act1 = rpm.vector.x;
  rpm_act2 = rpm.vector.y;
  rpm_dt = rpm.vector.z;
  rpm_time = rpm.header.stamp;
}

void handle_gyro( const geometry_msgs::Vector3& gyro) {
  gyro_x = gyro.x;
  gyro_y = gyro.y;
  gyro_z = gyro.z;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "base_controller");

  ros::NodeHandle n;
  ros::NodeHandle nh_private_("~");
  ros::Subscriber sub = n.subscribe("rpm", 50, handle_rpm);
  ros::Subscriber gyro_sub = n.subscribe("gyro", 50, handle_gyro);
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster broadcaster;

  double rate = 10.0;
  double linear_scale_positive = 1.0;
  double linear_scale_negative = 1.0;
  double angular_scale_positive = 1.0;
  double angular_scale_negative = 1.0;
  double angular_scale_accel = 1.0;
  double acc_theta = 0.0;
  double acc_x = 0.0;
  double acc_max_theta = 0.0;
  double acc_max_x = 0.0;
  double alpha = 0.0;
  bool publish_tf = false;//whether to publish tf or not. set to false if you want to use robot_pose_ekf
  bool use_imu = false;
  double dt = 0.0;
  double dx = 0.0;
  double dy = 0.0;
  double dth_odom = 0.0;
  double dth_gyro = 0.0;
  double dth = 0.0;
  double dth_prev = 0.0;
  double dth_curr = 0.0;
  double dxy_prev = 0.0;
  double dxy_ave = 0.0;
  double vx = 0.0;
  double vy = 0.0;
  double vth = 0.0;
  char base_link[] = "/base_link";
  char odom[] = "/odom";
  ros::Duration d(1.0);
  nh_private_.getParam("publish_rate", rate);
  nh_private_.getParam("publish_tf", publish_tf);
  nh_private_.getParam("linear_scale_positive", linear_scale_positive);
  nh_private_.getParam("linear_scale_negative", linear_scale_negative);
  nh_private_.getParam("angular_scale_positive", angular_scale_positive);
  nh_private_.getParam("angular_scale_negative", angular_scale_negative);
  nh_private_.getParam("angular_scale_accel", angular_scale_accel);
  nh_private_.getParam("alpha", alpha);
  nh_private_.getParam("use_imu", use_imu);

  ros::Rate r(rate);
  while(n.ok()){
    ros::spinOnce();
   // ros::topic::waitForMessage<geometry_msgs::Vector3Stamped>("rpm", n, d);
    current_time = ros::Time::now();
    dt =(current_time-last_time).toSec();
    dxy_ave = (rpm_act1+rpm_act2)*dt/(60*2);    //speed
    dth_odom = ((rpm_act2-rpm_act1)*dt)/(60*track_width);

    if(dxy_ave>0.0)
    {
        ROS_INFO("dxy_ave:%f, dth_odom:%f",dxy_ave,dth_odom);
    }

//    if (use_imu) dth_gyro = dt*gyro_z;



    dth = dth_odom;


    if(dth>0.0)
    {
        ROS_INFO("dth:%f",dth);
    }


    if (dth > 0) dth *= 5.0; //angular_scale_positive
    if (dth < 0) dth *= -5.0; //angular_scale_negative
    if (dxy_ave > 0) dxy_ave *= linear_scale_positive;
   // if (dxy_ave > 0) dxy_ave *= linear_scale_negative;

    dx = cos(dth) * dxy_ave;
    dy = -sin(dth) * dxy_ave;


    x_pos += (cos(theta) * dx - sin(theta) * dy);
    y_pos += (sin(theta) * dx + cos(theta) * dy);
    theta += dth;
    if(dx>0.0 ...
(more)
edit retag flag offensive close merge delete

Comments

@kallivalli Actually you no need to post bunch of codes over here, you try to learn how to ask questions. You should only post codes related to the issue your facing.

ok fine. Lets come to problem, I noticed that in your ROS node (the line below)

if (dth < 0) dth *= -5.0; //angular_scale_negative

you are scaling a negative value by a negative number, resulting in a positive value and causing rotation in a common direction i.e.in your case anticlockwise direction Just remove the negative sign from your scaling factor

to_infinity_nbeyond gravatar imageto_infinity_nbeyond ( 2019-10-22 01:00:04 -0600 )edit