ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Turtlebot 1 Bumper and IR Sensor Problem

asked 2014-05-02 04:30:15 -0500

nadal11 gravatar image

updated 2014-05-15 01:20:18 -0500

Hi everybody,

Kindly please help me out here. I am trying to access to Bumper and IR sensors on turtlebot1 (roomba500 series). Just would like to know whether I need a launch file to access this sensors? If so,could anybody please advise or give me an example of the launch file. I also put a check point in the call back function to check whether it goes into the loop but it didn't print anything on my screen as shown below. Follow is my source code that I used:

#include <ros/ros.h>
#include <turtlebot_node/TurtlebotSensorState.h>
#include <geometry_msgs/Twist.h>
#include <ros/rate.h>
#include <iostream>
#include <stdio.h>                        

using namespace std;    
uint8_t bumper;

//define the bumperCallback function
void bumperCallback(const turtlebot_node::TurtlebotSensorState::ConstPtr& msg)
 {
   cout<<"Check point 11111111111111111"<<endl;
   bumper = msg->bumps_wheeldrops;
   ROS_INFO("bumper hit. value = [%d]", bumper);
 }

//ROS node entry point 
int main(int argc, char **argv)
{
   ros::init(argc, argv, "turtlebot_test_node");
   ros::NodeHandle n;

   ros::Subscriber bumperSubscriber = n.subscribe("/turtlebot_node/sensor_state", 1000, bumperCallback);
   ros::Publisher velocityPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

   //create a Twist message to publish the velocity
   geometry_msgs::Twist velMessage;
   velMessage.linear.x = 0.0;
   velMessage.angular.z = 0.0;

   //publish the message
   velocityPublisher.publish(velMessage);

   ros::spin();

   return 0;
}

Many thanks!

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-05-16 00:17:13 -0500

nadal11 gravatar image

updated 2014-05-16 00:19:11 -0500

I have figure it out. Instead of subscribe to turtlebot_node, I subscribe to roomba_500_series. Here is my modified code and it is working for me. Note that, turtlebot needs to connect to roomba in order to receive sensors data from bumper. roomba_500_series also publish sensors data from ir_bumper. The code is:

#include <ros/ros.h>
#include <roomba_500_series/Bumper.h>
#include <geometry_msgs/Twist.h>
#include <ros/rate.h>
#include <iostream>
#include <stdio.h>                        

using namespace std;    
bool left_bumper, right_bumper;

//define the bumperCallback function
void bumperCallback(const roomba_500_series::Bumper::ConstPtr& msg)
{
     cout<<"Check point 11111111111111111"<<endl;
     left_bumper = msg->left.state;
     right_bumper = msg->right.state;
     cout<<"left bumper been hitted. value = "<<left_bumper<<endl;
     cout<<"right bumper been hitted. value = "<<right_bumper<<endl;
}

//ROS node entry point 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "turtlebot_test_node");
    ros::NodeHandle n;

    ros::Subscriber bumperSubscriber = n.subscribe("/bumper", 1000, bumperCallback);
    ros::Publisher velocityPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

    //create a Twist message to publish the velocity
    geometry_msgs::Twist velMessage;
    velMessage.linear.x = 0.0;
    velMessage.angular.z = 0.0;

    //publish the message
    velocityPublisher.publish(velMessage);

   ros::spin();

   return 0;
}
edit flag offensive delete link more
0

answered 2014-05-14 06:41:04 -0500

ahendrix gravatar image

It sounds like your callback isn't getting called; either because you're not subscribing to the correct topic, or because nothing is being published.

I would start by making sure that the turtlebot driver is running before you start your node.

You can inspect the topic in question, to see if there are any publishers and subscribers, by running:

rostopic info /turtlebot_node/sensor_state

If the topic exists, you can confirm that messages are being published and view their contents with:

rostopic echo /turtlebot_node/sensor_state

If the topic doesn't exist, but the driver is running, you may have the wrong topic name. You can list available topics with:

rostopic list
edit flag offensive delete link more

Comments

Thanks mate. By make sure the turtlebot driver running, do you mean connect turtlebot to roomba? I have run the rostopic info and it shows that the topic has been subscribed but not publish. From there, what should I do?

nadal11 gravatar image nadal11  ( 2014-05-15 01:06:15 -0500 )edit

The turtlebot driver is a node that connects to the serial/SCI port on the roomba and publishes its sensor data as ROS topics, you should make sure that you have that node correctly configured and running; take a look at the wiki page for it: http://wiki.ros.org/turtlebot_node

ahendrix gravatar image ahendrix  ( 2014-05-15 06:55:59 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2014-05-02 04:30:15 -0500

Seen: 1,791 times

Last updated: May 16 '14