//callback that will run if the sensor value changes by more than the OnSensorChange trigger. //Index - Index of the sensor that generated the event, Value - the sensor read value int AnalogInputHandler(CPhidgetInterfaceKitHandle IFK, void *usrptr, int Index, int Value) { //sensorValue 0-1000 ==> 0-5V // Update power data if(batteryPort == Index && powerdata_pub) { corobot_msgs::PowerMsg powerdata; powerdata.volts = (float) (Value - 500) * 0.0734; //The Min and Max present here are for the Nimh battery as it is the only one type of battery sold with a Corobot at the moment powerdata.min_volt = 10.0; powerdata.max_volt = 14.2; powerdata_pub.publish(powerdata); } else if(irFrontPort == Index && irData_pub) // Update IR data { corobot_msgs::SensorMsg data; data.type = data.INFRARED_FRONT; data.index = Index; data.value = irVoltageToDistance((float) Value / 200.0); irData_pub.publish(data); } else if(irBackPort == Index && irData_pub) // Update IR data { corobot_msgs::SensorMsg data; data.type = data.INFRARED_REAR; data.index = Index; data.value = irVoltageToDistance((float) Value / 200.0); irData_pub.publish(data); } //sonar else if(Index >= firstSonarInput && Index <= lastSonarInput && sonar_pub) { corobot_msgs::SensorMsg data; data.type = data.ULTRASOUND; data.index = Index; data.value = sonarVoltageToMeters((float) Value / 200.0); sonar_pub.publish(data); } else if(other_pub) // We don't know what sensor it is, but we publish { corobot_msgs::SensorMsg data; data.type = data.OTHER; data.value = Value; data.index = Index; other_pub.publish(data); } interfaceKitError = 0; return 0; }
/** * @brief Publish the data on their corresponding topics */ int publish_data(){ if(spatial_good) { corobot_msgs::spatial spatial; sensor_msgs::Imu imu; spatial.acc1 = acc[0]; spatial.acc2 = acc[1]; spatial.acc3 = acc[2]; spatial.ang1 = ang[0]; spatial.ang2 = ang[1]; spatial.ang3 = ang[2]; spatial.mag1 = mag[0]; spatial.mag2 = mag[1]; spatial.mag3 = mag[2]; imu.header.frame_id = "base_link"; imu.header.stamp = ros::Time::now(); imu.orientation = tf::createQuaternionMsgFromRollPitchYaw((atan2(dCMMatrix[2][1],dCMMatrix[2][2]) + rollOffset),(-asin(dCMMatrix[2][0]) + pitchOffset), (atan2(dCMMatrix[1][0],dCMMatrix[0][0]))); imu.angular_velocity.x = ang[0]; imu.angular_velocity.y = ang[1]; imu.angular_velocity.z = ang[2]; imu.linear_acceleration.x = acc[0]; imu.linear_acceleration.y = acc[1]; imu.linear_acceleration.z = acc[2]; spatial_pub.publish(spatial); imu_pub.publish(imu); spatialError = 0; } if(m_encodersGood) //the position is 4times the number of encoder counts. { corobot_msgs::PosMsg posdata; // prepare to read the encoder status int phidgetEncoderStatus = 0; CPhidget_getDeviceStatus((CPhidgetHandle) m_leftEncoder, &phidgetEncoderStatus); if (phidgetEncoderStatus != 0) { int value; CPhidgetEncoder_getPosition(m_leftEncoder, m_leftEncoderNumber, &value); posdata.px = -value; printf("Left encoder value = %d", value); } else { encoderError = 2; } phidgetEncoderStatus = 0; CPhidget_getDeviceStatus((CPhidgetHandle) m_rightEncoder, &phidgetEncoderStatus); if (phidgetEncoderStatus != 0) { int value; CPhidgetEncoder_getPosition(m_rightEncoder, m_rightEncoderNumber, &value); // we negate this value since the right side motors turn the opposite direction // to the left side posdata.py = value; } else { encoderError = 2; } posdata.header.stamp = ros::Time::now(); posdata_pub.publish(posdata); encoderError = 0; } else { encoderError = 1; } if (m_validAnalogs){ //////////////////////////// // Update power data if(batteryPort != -1) { corobot_msgs::PowerMsg powerdata; powerdata.volts = (float) (analog_inputs_value[batteryPort] - 500) * 0.0734; //The Min and Max present here are for the Nimh battery as it is the only one type of battery sold with a Corobot at the moment powerdata.min_volt = 10.0; powerdata.max_volt = 14.2; powerdata_pub.publish(powerdata); } //////////////////////////// // Update IR data if(irFrontPort != -1 || irBackPort != -1) { corobot_msgs::IrMsg irData; if(irFrontPort != -1) irData.voltage1=(float) analog_inputs_value[irFrontPort] / 200.0; else irData.voltage1=0; if(irBackPort != -1) irData.voltage2=(float) analog_inputs_value[irBackPort] / 200.0; else irData.voltage2=0; irData.range1=irVoltageToDistance(irData.voltage1); irData.range2=irVoltageToDistance(irData.voltage2); irData_pub.publish(irData); } if(gripperPort != -1) { corobot_msgs::GripperMsg gripperData; // Gripper connect to the 4th analog input port? gripperData.state=analog_inputs_value[gripperPort]; gripper_pub.publish(gripperData); } interfaceKitError = 0; } if (m_validDigitals) { //////////////////////////// // Update the bumper data corobot_msgs::BumperMsg bumper_data; if (m_rearBumperPresent) { bumper_data.bumpers_count=4; bumper_data.value0=(digital_inputs_value) & (0x01<<0); bumper_data.value1=(digital_inputs_value) & (0x01<<1); bumper_data.value2=(digital_inputs_value) & (0x01<<2); bumper_data.value3=(digital_inputs_value) & (0x01<<3); } else { bumper_data.bumpers_count=2; bumper_data.value0=(digital_inputs_value) & (0x01<<0); bumper_data.value1=(digital_inputs_value) & (0x01<<1); } bumper_pub.publish(bumper_data); interfaceKitError = 0; } return 0; }