Robotics StackExchange | Archived questions

publishing issue

I want to publish a ultrasonic distance from arduino.The ultra sonic sensor is acting as a radar with the help of a servo motor.But i getting some out of sync messages when i echoed to the topic range data.The code is given below.can any one help to correct this issue.

code:

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
 #include <Servo.h>. 
 ros::NodeHandle nh;
    // Defines Tirg and Echo pins of the Ultrasonic Sensor
    const int trigPin = 2;
    const int echoPin = 4;
    int i=0;
    double r =0;
double d;
int j;

    // Variables for the duration and the distance
    double duration;
    double distance;
    sensor_msgs::Range range_msg;
    Servo myServo; 
    ros::Publisher pub_range( "range_data", &range_msg);
    char frameid[] = "range_data";// Creates a servo object for controlling the servo motor
    void setup() {
      Serial.begin(57600);
nh.initNode();
nh.advertise(pub_range);
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.field_of_view = 0.8;
range_msg.min_range = 0.0;
range_msg.max_range = 400;
      pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
      pinMode(echoPin, INPUT); // Sets the echoPin as an Input
      //Serial.begin(9600);
      myServo.attach(8); // Defines on which pin is the servo motor attached
    }
    double getRange_Ultrasound(){
      // rotates the servo motor from 15 to 165 degrees
      digitalWrite(trigPin, LOW); 
      delayMicroseconds(2);
      // Sets the trigPin on HIGH state for 10 micro seconds
      digitalWrite(trigPin, HIGH); 
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
      distance= (duration*343)/20000;
      if (distance>500)
      {
        distance=400;
      }
      Serial.print(distance); // Sends the distance value into the Serial Port
        Serial.println();
      return distance;
     delay(50);
    }
    // Function for calculating the distance measured by the Ultrasonic sensor

    double range_time;
    void loop() {
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
//if ( millis() >= range_time ){

for( i=0;i<=180;i++){  
 j=i-1;
      myServo.write(i);
      //delay(5);
range_msg.range = getRange_Ultrasound();
if (range_msg.range<90)
  {
      myServo.detach();
     // delay(10);
      i=j;
  }
  else
  {
     myServo.attach(8);
  }
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
      //distance = calculateDistancf e();// Calls a function for calculating the distance measured by the Ultrasonic sensor for each degree

      //Serial.print(i); // Sends the current degree into the Serial Port
      //Serial.print(","); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
      //Serial.print(distance); // Sends the distance value into the Serial Port
        //Serial.println();
        //return (distance);
      //Serial.print("."); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
      }

      // Repeats the previous lines from 165 to 15 degrees
      for(i=180;i>=0;i--){  
        j=i+1;
      myServo.write(i);
      range_msg.range = getRange_Ultrasound();
      if (range_msg.range<90)
  {
      myServo.detach();
    //  delay(10);
      i=j;
  }
  else
  {
     myServo.attach(8);
  }
  range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
//range_msg.header.stamp = nh.now();
//pub_range.publish(&range_msg); 


     // delay(5);
      //distance = calculateDistance();

      //Serial.print(i);
      //Serial.print(",");
     // Serial.print(distance);
       // Serial.println();
       // return distance;
      //Serial.print(".");
      }

if ( millis() >= range_time ){

range_time =millis() + 50;
}
nh.spinOnce();
delay(200);
}

I wrote a node in ros to subscribe to this topic and perform a path planning operation on abb 2400 in rviz.But while running this node, after a few second i got segmentation fault core dumped message .How to resolve this issue The code is:

include

#include #include "sensor_msgs/Range.h"

include "std_msgs/Bool.h"

//#include

include

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

//#include

//ros::NodeHandle n;

//#include

float range=1 ; //range=1000; int r=1; int i=1; int j=2; //int i=1; void rangeCallback(const sensormsgs::Range::ConstPtr& rangedata) {

//sleep(5); range = rangedata->range; ROSINFO("I heard: [%f]", range); ros::AsyncSpinner spinner(1); spinner.start();

moveit::planning_interface::MoveGroup group("manipulator"); // ros::Rate r(1);

geometrymsgs::Pose targetpose1; geometrymsgs::Pose targetpose2; geometrymsgs::Pose targetpose3; geometrymsgs::Pose targetpose4; geometrymsgs::Pose targetpose5; geometrymsgs::Pose targetpose6; geometrymsgs::Pose targetpose7; geometrymsgs::Pose targetpose8; geometrymsgs::Pose targetpose9; geometrymsgs::Pose targetpose10; geometrymsgs::Pose targetpose11; geometrymsgs::Pose targetpose12; geometrymsgs::Pose targetpose13; geometrymsgs::Pose targetpose14; geometrymsgs::Pose targetpose15; geometrymsgs::Pose targetpose16; geometrymsgs::Pose targetpose17; //sleep(50); //i=i+1; goto l1; l1:if(range>60) { //sleep(1); //sleep(100); switch(i) { case 1:

target_pose1.orientation.w = 0.70714;

target_pose1.orientation.x= -2.7274e-05;

target_pose1.orientation.y = 0.70707;

target_pose1.orientation.z = 5.5968e-06;

target_pose1.position.x = 0.94004;

target_pose1.position.y = 4.3703e-05;

target_pose1.position.z = 1.455;

group.setPoseTarget(target_pose1);

///Motion plan from current location to custom position

//success = group.plan(my_plan); group.asyncMove();

//ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); break; case 2:

/*target_pose2.orientation.w = 0.70711;

target_pose2.orientation.x= 7.1913e-05;

target_pose2.orientation.y = 0.7071;

target_pose2.orientation.z = 2.8617e-06;

target_pose2.position.x = 0.94;

target_pose2.position.y = -7.8182e-05;

targetpose2.position.z = 0.99918;*/ targetpose2.orientation.w = 0.70718;

target_pose2.orientation.x= -9.8623e-06;

target_pose2.orientation.y = 0.70703;

target_pose2.orientation.z = 1.5462e-05;//-4.0823e-05;

target_pose2.position.x = 0.98928;

target_pose2.position.y = 0.026022;

target_pose2.position.z = 1.4017;

group.setPoseTarget(target_pose2);

///Motion plan from current location to custom position

//moveit::planninginterface::MoveGroup::Plan myplan2;

//success = group.plan(my_plan); group.asyncMove();

//ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");

/* Sleep to give RViz time to visualize the plan. */

//sleep(1.0); break;

case 3: target_pose3.position.x = 1.0105;

target_pose3.position.y = 0.035057;

target_pose3.position.z =1.3749;

target_pose3.orientation.x=-6.647e-05 ;

target_pose3.orientation.y = .70707;

target_pose3.orientation.z = -2.6612e-05;

target_pose3.orientation.w = 0.70715 ;

group.setPoseTarget(target_pose3);

group.asyncMove(); //success = group.plan(my_plan); //sleep(1.0);

break;

case 4: target_pose4.position.x = 1.0505;

target_pose4.position.y =0.050421 ;

target_pose4.position.z =1.3234 ;

target_pose4.orientation.x= -4.5034e-05;

target_pose4.orientation.y = 0.70716;

target_pose4.orientation.z = -4.398e-05;

target_pose4.orientation.w = 0.70705;

group.setPoseTarget(target_pose4);

group.asyncMove(); // success = group.plan(my_plan); // sleep(1.0);

break;

case 5: target_pose5.position.x = 1.0875;

target_pose5.position.y =0.062713 ;

target_pose5.position.z =1.2719;

target_pose5.orientation.x= -6.6919e-05;

target_pose5.orientation.y = 0.70713;

target_pose5.orientation.z = -2.3095e-06;

target_pose5.orientation.w = 0.70709;

group.setPoseTarget(target_pose5);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 6: target_pose6.position.x =1.1252;

target_pose6.position.y =0.06891 ;

target_pose6.position.z =1.2087;

target_pose6.orientation.x= -6.1468e-05;

target_pose6.orientation.y = 0.70711;

target_pose6.orientation.z = 2.6618e-05;

target_pose6.orientation.w = 0.7071;

group.setPoseTarget(target_pose6);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 7: target_pose7.position.x = 1.2178;

target_pose7.position.y =0.093716 ;

target_pose7.position.z =1.0699;

target_pose7.orientation.x=3.46e-05 ;

target_pose7.orientation.y = 0.70711;

target_pose7.orientation.z = 2.8208e-05;

target_pose7.orientation.w = 0.7071;

group.setPoseTarget(target_pose7);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 8: target_pose8.position.x =1.2695 ;

target_pose8.position.y =0.11545 ;

target_pose8.position.z =1.0064;

target_pose8.orientation.x= 3.2059e-05;

target_pose8.orientation.y = 0.70713;

target_pose8.orientation.z = 5.2586e-05;

target_pose8.orientation.w = 0.70708 ;

group.setPoseTarget(target_pose8);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 9: target_pose9.position.x =1.3182 ;

target_pose9.position.y =0.13401 ;

target_pose9.position.z =0.94327;

target_pose9.orientation.x= 1.6668e-05;

target_pose9.orientation.y = 0.70707;

target_pose9.orientation.z = 1.9536e-05;

target_pose9.orientation.w = 0.70714;

group.setPoseTarget(target_pose9);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 10: target_pose10.position.x =1.3621;

target_pose10.position.y = 0.15151;

target_pose10.position.z =0.88755;

target_pose10.orientation.x= -3.9888e-05;

target_pose10.orientation.y =0.70721;

target_pose10.orientation.z = 1.6248e-05;

target_pose10.orientation.w = 0.70701;

group.setPoseTarget(target_pose10);

group.asyncMove(); // success = group.plan(my_plan); // sleep(1.0);

break;

case 11: target_pose11.position.x = 1.4007;

target_pose11.position.y =0.15657 ;

target_pose11.position.z =0.82036;

target_pose11.orientation.x= -3.906e-05;

target_pose11.orientation.y = 0.70711;

target_pose11.orientation.z = 3.0171e-05;

target_pose11.orientation.w = 0.70711 ;

group.setPoseTarget(target_pose11);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 12: target_pose12.position.x =1.4369;

target_pose12.position.y = 0.15234;

target_pose12.position.z =0.74127;

target_pose12.orientation.x= -4.2897e-05;

target_pose12.orientation.y = 0.70709;

target_pose12.orientation.z = -6.3212e-06;

target_pose12.orientation.w = 0.70712;

group.setPoseTarget(target_pose12);

group.asyncMove(); // success = group.plan(my_plan);
// sleep(1.0);

break;

case 13: target_pose13.position.x =1.4756 ;

target_pose13.position.y = 0.15731;

target_pose13.position.z =0.67403;

target_pose13.orientation.x= -4.1473e-05;

target_pose13.orientation.y = 0.70712;

target_pose13.orientation.z =3.1461e-06 ;

target_pose13.orientation.w = 0.70709;

group.setPoseTarget(target_pose13);

group.asyncMove(); // success = group.plan(my_plan);
// sleep(1.0);

break;

case 14: target_pose14.position.x =1.5101;

target_pose14.position.y = 0.16016;

target_pose14.position.z =0.61055;

target_pose14.orientation.x= -1.0851e-05;

target_pose14.orientation.y = 0.70718;

target_pose14.orientation.z = 2.68e-06;

target_pose14.orientation.w = 0.70704 ;

group.setPoseTarget(target_pose14);

group.asyncMove(); // success = group.plan(my_plan); // sleep(1.0);

break;

case 15: target_pose15.position.x = 1.5572;

target_pose15.position.y = 0.16339;

target_pose15.position.z =0.52343;

target_pose15.orientation.x=-4.8364e-05;

target_pose15.orientation.y =0.70716;

target_pose15.orientation.z =-2.1301e-05;

target_pose15.orientation.w = 0.70705 ;

group.setPoseTarget(targetpose15); //success = group.plan(myplan); group.asyncMove();

//sleep(1.0);

break;

case 16: target_pose16.position.x = 1.5998;

target_pose16.position.y =0.17053 ;

target_pose16.position.z =0.45197;

target_pose16.orientation.x= 5.3451e-05;

target_pose16.orientation.y = 0.70718;

target_pose16.orientation.z =1.2117e-05 ;

target_pose16.orientation.w = 0.70704;

group.setPoseTarget(target_pose16);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

case 17: target_pose17.position.x =1.611 ;

target_pose17.position.y = 0.16072;

target_pose17.position.z =0.4124;

target_pose17.orientation.x=-3.7932e-05;

target_pose17.orientation.y = 0.70701;

target_pose17.orientation.z = 2.1195e-05;

target_pose17.orientation.w = 0.70711;

group.setPoseTarget(target_pose17);

group.asyncMove(); // success = group.plan(my_plan); //sleep(1.0);

break;

default: group.stop(); break; }

i=i+j; if(i>17) { j=-2; i=16; } if(i<2) { j=2; i=2; } } else { //sleep(50); group.setMaxAccelerationScalingFactor(0.1); group.setMaxVelocityScalingFactor(0.1); group.stop(); //goto l1; sleep(4); }

//ros::spin();

} int main( int argc, char** argv ) { ros::init(argc, argv, "basic_shapes2"); ros::NodeHandle m; ros::Rate r(1);

ros::Subscriber sub = m.subscribe("rangedata", 9600,rangeCallback);
//ros::Subscriber sub1 = m.subscribe("range
data1", 9600,rangeCallback);
ros::spin(); return 0; //r.sleep(); }

But i am getting segmentation fault orcoredumped message after a few seconds i ran this ros node

Asked by anadgopi1994 on 2016-11-23 23:10:35 UTC

Comments

I wrote a node in ros to subscribe to this topic and perform a path planning operation on abb 2400 in rviz.But while running this node, after a few second i got segmentation fault core dumped message .How to resolve this issue

Asked by anadgopi1994 on 2016-11-24 22:46:29 UTC

Answers