int main(int argc, char* argv[])
{
ros::init(argc, argv, "phidget_component");
ros::NodeHandle n;
ros::NodeHandle nh("~");
nh.param("rearBumper", m_rearBumperPresent, false);
nh.param("bwOutput", bwOutput, -1);
nh.param("strobeOutput", strobeOutput, -1);
nh.param("lastSonarInput", lastSonarInput, -1);
nh.param("firstSonarInput", firstSonarInput, -1);
//index of the battery voltage sensor
nh.param("battery", batteryPort, 0);
// index of the front ir sensor
nh.param("irFront", irFrontPort, 1);
//index of the back ir sensor
nh.param("irBack", irBackPort, 2);
nh.param("imu", imu, true);
//create an updater that will send information on the diagnostics topics
diagnostic_updater::Updater updater;
updater.setHardwareIDf("Phidget");
//function that will be executed with updater.update()
updater.add("Interface Kit", phidget_ik_diagnostic);
//function that will be executed with updater.update()
updater.add("Spatial", phidget_spatial_diagnostic);
interfacekit_simple();
ros::Rate rate(70);
while (ros::ok())
{
// ROS loop
ros::spinOnce();
// acquire new sonar data if sonar sensors are present
if(sonarsPresent)
sendSonarResult();
//update diagnostics
updater.update();
rate.sleep();
}
// close all the phidget devices
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
if (imu)
{
CPhidget_close((CPhidgetHandle)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
}
return 0;
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "phidget_component");
ros::NodeHandle n;
ros::NodeHandle nh("~");
nh.param("rearBumper", m_rearBumperPresent, false);
nh.param("bwOutput", bwOutput, -1);
nh.param("strobeOutput", strobeOutput, -1);
nh.param("lastSonarInput", lastSonarInput, -1);
nh.param("firstSonarInput", firstSonarInput, -1);
nh.param("battery", batteryPort, 0);
nh.param("irFront", irFrontPort, 1);
nh.param("irBack", irBackPort, 2);
nh.param("gripper", gripperPort, 3);
nh.param("motors_inverted", motors_inverted, false);
nh.param("encoders_inverted", encoders_inverted, false);
dCMMatrix[0][0]=1;
dCMMatrix[0][1]=0;
dCMMatrix[0][2]=0;
dCMMatrix[1][0]=0;
dCMMatrix[1][1]=1;
dCMMatrix[1][2]=0;
dCMMatrix[2][0]=0;
dCMMatrix[2][1]=0;
dCMMatrix[2][2]=1;
timestampPreviousCall = 0;
pitchOffset = 0;
rollOffset = 0;
proportionalVector[0] = 0;
proportionalVector[1] = 0;
proportionalVector[2] = 0;
integratorVector[0] = 0;
integratorVector[1] = 0;
integratorVector[2] = 0;
gyroscopeVectorCorrected[0] = 0;
gyroscopeVectorCorrected[1] = 0;
gyroscopeVectorCorrected[2] = 0;
//create an updater that will send information on the diagnostics topics
diagnostic_updater::Updater updater;
updater.setHardwareIDf("Phidget");
updater.add("Interface Kit", phidget_ik_diagnostic); //function that will be executed with updater.update()
updater.add("Spatial", phidget_spatial_diagnostic); //function that will be executed with updater.update()
updater.add("Encoders", phidget_encoder_diagnostic); //function that will be executed with updater.update()
interfacekit_simple();
ros::ServiceServer odom = n.advertiseService("phidgetnode_set_odom", SetOdom);
posdata_pub = n.advertise<corobot_msgs::PosMsg>("position_data", 100);
irData_pub = n.advertise<corobot_msgs::IrMsg>("infrared_data", 100);
powerdata_pub = n.advertise<corobot_msgs::PowerMsg>("power_data", 100);
bumper_pub = n.advertise<corobot_msgs::BumperMsg>("bumper_data", 100);
gripper_pub = n.advertise<corobot_msgs::GripperMsg>("gripper_data",100); //gripper as an analog input?
sonar_pub = n.advertise<corobot_msgs::RangeSensor>("sonar_data", 100);
spatial_pub = n.advertise<corobot_msgs::spatial>("spatial_data",100);
imu_pub = n.advertise<sensor_msgs::Imu>("imu_data",100);
ros::Publisher phidget_info_pub = n.advertise<corobot_msgs::phidget_info>("phidget_info", 1000);
corobot_msgs::phidget_info info;
if(phidget888_connected)
info.phidget888_connected = 1;
else info.phidget888_connected = 0;
if(phidget_encoder_connected)
info.phidget_encoder_connected = 1;
else info.phidget_encoder_connected = 0;
phidget_info_pub.publish(info);
ros::Rate loop_rate(20); //20 Hz
while (ros::ok())
{
ros::spinOnce();
if(sonarsPresent)
sendSonarResult();
publish_data();
loop_rate.sleep();
updater.update();
}
printf("Out of ros spin");
CPhidget_close((CPhidgetHandle)ifKit);
CPhidget_delete((CPhidgetHandle)ifKit);
if (m_rightEncoder != m_leftEncoder)
{
CPhidget_close((CPhidgetHandle)m_leftEncoder);
CPhidget_delete((CPhidgetHandle)m_leftEncoder);
}
CPhidget_close((CPhidgetHandle)m_rightEncoder);
CPhidget_delete((CPhidgetHandle)m_rightEncoder);
CPhidget_close((CPhidgetHandle)spatial);
CPhidget_delete((CPhidgetHandle)spatial);
return 0;
}
