I am publishing a PIR (Passive Infrared ) sensor data using Arduino ROS node. I wrote another ROS node which is given below, which subscribes to this PIR data send from the Arduino. This node tells a ABB 2400 to execute a path in a loop similar to a pick and place operating path. Based on the sensor values, I am slowing down the robot which is executing a continuous path. But a "Segmentation fault" error is showing after the the robot completes one or two loops of execution. Can any one please answer me how to solve this problem. I am giving my ROS code below:
float range;
float sx=0.750045;
float sy;
float sz=1.17302;
int i=1;
int j=1;
int c=1;
float k=-0.005;
int n=1;
float dx;
float cz1;
float cz2;
float cx1;
float cx2;
geometry_msgs::Pose target_pose1;
geometry_msgs::Pose target_pose2;
geometry_msgs::Pose target_pose3;
geometry_msgs::Pose target_current;
void rangeCallback(const std_msgs::Float32::ConstPtr& pir_data)
{
range=pir_data->data;
moveit::planning_interface::MoveGroup group("manipulator");
if(i<=1)
{
if(range==1)
{
group.setMaxAccelerationScalingFactor(0.005);
group.setMaxVelocityScalingFactor(0.005);
}
else
{
group.setMaxAccelerationScalingFactor(0.05);
group.setMaxVelocityScalingFactor(0.05);
}
target_pose1.orientation.w = 0.162635;
target_pose1.orientation.x= 0.0283592;
target_pose1.orientation.y = 0.985703;
target_pose1.orientation.z = 0.0336909;
target_pose1.position.x = 0.769102;
target_pose1.position.y = -0.0173314;
target_pose1.position.z = 0.915313;
group.setPoseTarget(target_pose1);
group.asyncMove();
if(range==0)
{
sleep(2.2);
}
else
{
sleep(6);
}
target_current=group.getCurrentPose().pose;
cz1=target_pose1.position.z;
cz2=target_current.position.z;
cz1=floorf(cz1*100)/100;
cz2=floorf(cz2*100)/100;
cx1=target_pose1.position.x;
cx2=target_current.position.x;
cx1=floorf(cx1*100)/100;
cx2=floorf(cx2*100)/100;
if(cz1==cz2 && cx1==cx2)
{
i=i+1;
j=1;
}
else
{
i=1;
c=c+1;
if(c==3)
{
i=i+1;
c=1;
}
}
}
else if(i==2)
{
if(range==1)
{
group.setMaxAccelerationScalingFactor(0.005);
group.setMaxVelocityScalingFactor(0.005);
}
else
{
group.setMaxAccelerationScalingFactor(0.05);
group.setMaxVelocityScalingFactor(0.05);
}
target_pose2.orientation.w = 0.160792;
target_pose2.orientation.x= 0.0376416;
target_pose2.orientation.y = 0.985994;
target_pose2.orientation.z = -0.0233425;
target_pose2.position.x = 0.608187;
target_pose2.position.y = -0.146997;
target_pose2.position.z = 1.01364;
group.setPoseTarget(target_pose2);
group.asyncMove();
if(range==0)
{
sleep(2.2);
}
else
{
sleep(6);
}
target_current=group.getCurrentPose().pose;
cz1=target_pose2.position.z;
cz2=target_current.position.z;
cz1=floorf(cz1*100)/100;
cz2=floorf(cz2*100)/100;
cx1=target_pose2.position.x;
cx2=target_current.position.x;
cx1=floorf(cx1*100)/100;
cx2=floorf(cx2*100)/100;
if(cz1==cz2 && cx1==cx2)
{
if(j==1)
{
i=3;
}
else
{
i=1;
}
}
else
{
i=2;
c=c+1;
if(c==3)
{
if(j==1)
{
i=3;
}
else
{
i=1;
}
c=1;
}
}
}
else
{
if(range==1)
{
group.setMaxAccelerationScalingFactor(0.005);
group.setMaxVelocityScalingFactor(0.005);
//group.stop();
}
else
{
group.setMaxAccelerationScalingFactor(0.05);
group.setMaxVelocityScalingFactor(0.05);
}
target_pose3.orientation.w = 0.160727;
target_pose3.orientation.x= 0.0376638;
target_pose3.orientation.y = 0.986003;
target_pose3.orientation.z = -0.0233583;
target_pose3.position.x = 0.613978;
target_pose3.position.y = -0.284084;
target_pose3.position.z = 0.862117;
group.setPoseTarget(target_pose3);
group.asyncMove();
if(range==0)
{
sleep(2.2);
}
else
{
sleep(6);
}
target_current=group.getCurrentPose().pose;
cz1=target_pose3.position.z;
cz2=target_current.position.z;
cz1=floorf(cz1*100)/100;
cz2=floorf(cz2*100)/100;
cx1=target_pose3.position.x;
cx2=target_current.position.x;
cx1=floorf(cx1*100)/100;
cx2=floorf(cx2*100)/100;
if(cz1==cz2 && cx1==cx2)
{
i=i-1;
j=2;
}
else
{
i=3;
c=c+1;
if(c==3)
{
i=i-1;
j=2;
c=1;
}
}
}
ros::spin();
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "goal");
ros::NodeHandle m;
ros::Rate r(1);
ros::Subscriber sub = m.subscribe("pir_data",1000,rangeCallback);
ros::spin();
return 0;
}
↧