void* bacnet_data(void* adapter)
{
pthread_mutex_lock(mio_lock);
while(TRUE)
{
publish_data(adapter);
pthread_mutex_unlock(mio_lock);
sleep(10);
pthread_mutex_lock(mio_lock);
}
return NULL;
}
void readFrontCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
//create a pcl point cloud first...
pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>);
//now there is some processing neccessary...convert msg to pcl point cloud
pcl::fromROSMsg(*msg, *input);
//apply ransac plane removal:
input=remove_ground_plane(input);
//now remove outliers:
input= apply_outlier_filter(input, neigbour_nr,radius);
//cluter clouds with the given distance,and get out the center of every cluster
input=cluster_clouds(input,distance);
publish_data(input,msg->header.frame_id);
}
void FC_Mpkg::handle_input(const mavlink_message_t &msg) {
// vector<int> v(16);
//mavlink_message_t msg_j;
//Logger::log("FC_Mpkg got mavlink_message [len, id]:", (int)msg.len, (int)msg.msgid, Logger::LOGLEVEL_DEBUG);
if(msg.msgid == MAVLINK_MSG_ID_MK_DEBUGOUT) {
//Logger::log("FC_Mpkg got MK_DEBUGOUT", Logger::LOGLEVEL_INFO);
mavlink_msg_mk_debugout_decode(&msg, (mavlink_mk_debugout_t *)&mk_debugout);
// MK FlightCtrl IMU data
debugout2attitude(&mk_debugout, &huch_attitude);
// MK FlightCtrl Barometric sensor data
debugout2altitude(&mk_debugout, &huch_altitude);
// MK huch-FlightCtrl I2C USS data
// XXX: this should be in kopter config, e.g. if settings == fc_has_uss
debugout2ranger(&mk_debugout, &huch_ranger);
// real debugout data
debugout2status(&mk_debugout, &mk_fc_status);
// put into standard pixhawk structs
// raw IMU
//set_pxh_raw_imu();
set_pxh_attitude();
set_pxh_manual_control();
publish_data(get_time_us());
// deadlock problem
// mavlink_msg_huch_attitude_encode(42, 23, &msg_j, &huch_attitude);
// send(msg_j);
}
// // compare with qk_datatypes
// mk_debugout_o = (DebugOut_t *)&mk_debugout;
// v[0] = (int16_t)mk_debugout_o->Analog[ADval_accnick];
// v[1] = (int16_t)mk_debugout_o->Analog[ADval_accroll];
// v[2] = (int16_t)mk_debugout_o->Analog[ADval_acctop];
// v[3] = (int16_t)mk_debugout_o->Analog[ADval_acctopraw];
// v[4] = (int16_t)mk_debugout_o->Analog[ATTmeanaccnick];
// v[5] = (int16_t)mk_debugout_o->Analog[ATTmeanaccroll];
// v[6] = (int16_t)mk_debugout_o->Analog[ATTmeanacctop];
// v[7] = (int16_t)mk_debugout_o->Analog[ADval_gyrnick];
// v[8] = (int16_t)mk_debugout_o->Analog[ADval_gyrroll];
// v[9] = (int16_t)mk_debugout_o->Analog[ADval_gyryaw];
// Logger::log("FC_Mpkg decoded: ", v, Logger::LOGLEVEL_INFO);
// if(msg.sysid == system_id && msg.msgid == 0) {//FIXME: set right msgid
// //TODO
// }
}
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;
}
void SenCmp02::run() {
char buffer[256];
int wait_freq = update_rate? 1000000 / update_rate: 0;
int wait_time = wait_freq;
uint64_t frequency = wait_time;
uint64_t start = get_time_us();
uint64_t time_output = start + 1000000;
uint64_t usec;
vector<uint16_t> cmp_value(2);
//mavlink_message_t msg;
Logger::log("Cmp02: running (Hz)", update_rate, Logger::LOGLEVEL_INFO);
// uint64_t end = getTimeUs() + 1000000;
status = RUNNING;
while((status == RUNNING) && update_rate) {
try {
/* wait time */
usec = get_time_us();
uint64_t end = usec;
wait_time = wait_freq - (end - start);
wait_time = (wait_time < 0)? 0: wait_time;
/* wait */
usleep(wait_time);
//usleep(10);
/* calculate frequency */
end = get_time_us();
frequency = (15 * frequency + end - start) / 16;
start = end;
// Logger::log("sencmp2: pre i2c_start_conversion", Logger::LOGLEVEL_DEBUG);
i2c_start_conversion(fd, CMP02_ADR);
// set read register
buffer[0] = 0x00;
// Logger::log("sencmp2: pre i2c_write_bytes", Logger::LOGLEVEL_DEBUG);
i2c_write_bytes(fd, (uint8_t*)buffer, 1);
// read data
// Logger::log("sencmp2: pre i2c_read_bytes", Logger::LOGLEVEL_DEBUG);
i2c_read_bytes(fd, (uint8_t*)buffer, 1);
// get version
version = *buffer;
// Logger::log("Cmp02: version", (int)version, Logger::LOGLEVEL_INFO);
buffer[0] = 0x01;
// Logger::log("sencmp2: pre i2c_write_bytes 2", Logger::LOGLEVEL_DEBUG);
i2c_write_bytes(fd, (uint8_t*)buffer, 1);
// Logger::log("sencmp2: pre i2c_read_bytes 2", Logger::LOGLEVEL_DEBUG);
i2c_read_bytes(fd, (uint8_t*)buffer, 1);
// Logger::log("sencmp2: pre i2c_end_conversion", Logger::LOGLEVEL_DEBUG);
i2c_end_conversion(fd);
// get values
memcpy(&cmp_value[0], buffer, 1);
// Logger::log("Cmp02: value", (int)cmp_value[0], Logger::LOGLEVEL_INFO);
// FIXME: smart++
// exp_ctrl_rx_data.value0 = cmp_value[0];
// exp_ctrl_rx_data.value1 = cmp_value[1];
// exp_ctrl_rx_data.value2 = cmp_value[2];
// exp_ctrl_rx_data.value3 = cmp_value[3];
// FIXME: kopter specific mapping
// FIXME: 0 is USS
// huch_ranger.ranger2 = cmp_value[2];
// huch_ranger.ranger3 = cmp_value[0];
// home / qk01
// huch_ranger.ranger2 = cmp_value[0];
// huch_ranger.ranger3 = cmp_value[1];
/* assign buffer to data */
#ifdef MAVLINK_ENABLED_HUCH
{ // begin of data mutex scope
int i;
cpp_pthread::Lock ri_lock(data_mutex);
for(i=0; i < CMP02_NUMCHAN; i++) {
sensor_data[i].analog = cmp_value[i];
sensor_data[i].usec = start;
// Logger::log("Cmp02 sensor:", i, sensor_data[i].analog, Logger::LOGLEVEL_INFO);
}
} // end of data mutex scope
#endif // MAVLINK_ENABLED_HUCH
// FIXME: if(publish) else poll or whatever
publish_data(start);
//Logger::log("Cmp02:", (int)exp_ctrl_rx_data.version, cmp_value, Logger::LOGLEVEL_INFO);
//Logger::log("Cmp02:", DataCenter::get_sensor(chanmap[0]), Logger::LOGLEVEL_INFO);
// pass more data
// FIXME: system_id
// mavlink_msg_huch_exp_ctrl_rx_encode(39, static_cast<uint8_t>(component_id), &msg, &exp_ctrl_rx_data);
/* debug data */
if (debug) print_debug();
/* timings/benchmark output */
if (timings) {
if (time_output <= end) {
Logger::log("Cmp02 frequency: ", (float)1000000/frequency, Logger::LOGLEVEL_DEBUG);
time_output += 1000000;
}
}
} // end try
catch(const char *message) {
i2c_end_conversion(fd);
status = STRANGE;
string s(message);
Logger::log("sencmp2: would-be exception:", s, Logger::LOGLEVEL_DEBUG);
// fallback termination and free bus
i2c_end_conversion(fd);
Logger::log("sencmp2: would-be exception:", s, Logger::LOGLEVEL_DEBUG);
// FIXME: throw and exit on "write_bytes 2"
// throw ("Cmp02::run(): " + s).c_str();
status = RUNNING;
}
} // end while
Logger::debug("sencmp02: stopped, exiting run()");
return;
}
