use pointer to odometry message

asked 2018-07-01 07:45:34 -0600

conect gravatar image

updated 2018-07-01 08:21:50 -0600

I've been stuck on a problem for quite a while now. I'm trying to use multiple files (main function file, file containing different movement control functions, file for controlling the camera, etc... and header files) in order to make control my turtlebot. When I only use publishers and don't bother subscribing to any nodes, everything seems to work fine, including moving the turtlebot solely with publish commands, however when I try and use a subscriber to change what the motion will be depending on speed everything seems to fail.

My CMake list is fine and everything compiles correctly, but when I run it I get the following error:

/usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<t>::type boost::shared_ptr<t>::operator->() const [with T = const nav_msgs::Odometry_<std::allocator<void>

; typename boost::detail::sp_member_access<t>::type = const nav_msgs::Odometry_<std::allocator<void> *]: Assertion `px != 0' failed. Aborted (core dumped)

What I've figured out is that my odometry pointer (odom_ptr) seems to not be initializing, but I'm not sure why or how to fix it. Any help would be greatly appreciated.

For this I'm using the following piece of code:

MainFile.cpp

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Empty.h>
#include "TurtleFunction.cpp"
#include "TurtleFunction.h"

#include <sstream>
#include <string>
#include <cmath>
#include <math.h>

using namespace std;

/* Define Global Variables
--------------------------*/
// ROS Publishers and Subscribers
ros::Publisher cmd_pub, reset_pub;
ros::Subscriber OdomSub;

// Pointers and Messages
geometry_msgs::Twist cmd_msg;
nav_msgs::Odometry::ConstPtr odom_ptr;
std_msgs::Empty reset_msg ;


int main(int argc, char** argv){

    // Initialize ROS
    ros::init(argc, argv, "test3");
    ros::NodeHandle nh;
    ros::spinOnce();

    // Define Global Variables for use
    cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop", 10);
    reset_pub = nh.advertise<std_msgs::Empty>("/mobile_base/commands/reset_odometry", 1);
    OdomSub = nh.subscribe<nav_msgs::Odometry>("/odom",50,processodom);

    ros::spinOnce();
    resetOdom(); // Reset the Odometry of the Turtlebot
    ros::spinOnce();

    // Define Initial Velocity Variables for TurtleBot
    double v = 0.1;
    double theta = 0.01;

    int i = 0;

    while(1){
        // Movement(v,theta);
        UnicycleModel();
        cout << "this is working" << i << endl;
        i++;
    }

    cout << "This worked" << endl;

    return 0;
}

Then I have the following function file:

TurtleFunction.cpp

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Empty.h>
#include "TurtleFunction.h"


#include <sstream>
#include <string>
#include <cmath>
#include <math.h>

using namespace std;

void Movement(double v,double theta){

    // geometry_msgs::Twist cmd_msg;

    cmd_msg.linear.x  = v;
    cout << "v = " << cmd_msg.linear.x << endl;
    cmd_msg.angular.z  = theta;
    cout << "theta = " << cmd_msg.angular.z << endl;
    cmd_pub.publish(cmd_msg); //send

    ros::spinOnce();
}

void UnicycleModel(){

    float pi = 3.14159265359 ;

    float pose_judah_x = odom_ptr->pose.pose.position.x ;
    float pose_judah_y = odom_ptr->pose.pose.position.y ;
    float orientationW = odom_ptr->pose.pose.orientation.w ;
    float orientationX = odom_ptr->pose.pose.orientation.x ;
    float orientationY = odom_ptr->pose.pose.orientation.y ;
    float orientationZ = odom_ptr->pose.pose.orientation.z ;


    float psi_rad = atan2(2*orientationW*orientationZ,1-2*orientationZ*orientationZ) ;
    float psi_deg = psi_rad*180/pi ;

    if ...
(more)
edit retag flag offensive close merge delete

Comments

It is likely that no messages have been processed for the /odom subscriber and the very first time you run UnicycleModel() your code tries to dereference the odom_ptr and crashes.

Also: I would recommend you rewrite your while loop to check ros::ok() and introduce a ros::Rate.

gvdhoorn gravatar imagegvdhoorn ( 2018-07-01 10:56:52 -0600 )edit

And a question: is there any reason you don't want to run the code in UnicycleModel() inside the processodom(..) callback? You have all the context there (ie: msg, time, etc), don't need any additional variables, and you avoid undersampling, aliasing and potentially losing events.

gvdhoorn gravatar imagegvdhoorn ( 2018-07-01 10:58:46 -0600 )edit

Yeah, the reasoning not to run UnicyleModel in the proccessodom is because it is only one of 3 different movement functions I may use. Since I am trying to make my code as modular as possible, I have multiple different movement functions in that function file so that I can simply swap a line of code

conect gravatar imageconect ( 2018-07-01 17:09:51 -0600 )edit

If I put the unicycle model code in the processodom I'd have to rewrite it each time which defeat the purpose.

How could I make sure that the subscriber gets a message first? I tried spinOnce already and nothing

And yeah, going to make the change on the while, it's just a placeholder. Thanks

conect gravatar imageconect ( 2018-07-01 17:10:19 -0600 )edit

so I tried moving UnicyleModel() into proccessodom() just to see if that would help at all any see if I could work from there to debug. I also added ros::spin() so that it would continue running. when I ran rqt_graph it found just the node "test3" and nothing else, no publishers or subscribers

conect gravatar imageconect ( 2018-07-02 04:53:38 -0600 )edit