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 "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("rangedata1", 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