///handler needed for odom message and velocity message
void qc_imu_imu_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){
(void) clientData; //no warning
IPC_RETURN_TYPE err = IPC_OK;
err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &imsg, sizeof(imsg));
newImuMessageAvailable = true;
IPC_freeByteArray(callData);
IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&imsg);
}
/* Message handler function - It gets called when we receive a message
* with the name "ListeningToAndroid"
*/
static void msgHandler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData) {
drive_arc_command_msg_type msg1;
double radius, time, speed;
char sender[20];
time_type timestamp;
IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData, &msg1, sizeof(msg1));
fp=fopen("CommandLog.txt","a");
fprintf(fp,"Command Radius: %f, Speed: %f, Time: %f",msg1.radius,msg1.speed,msg1.time);
fprintf(fp,"\tSender: %s",msg1.sender.data);
time_t commandTime=msg1.timestamp.tv_sec;
fprintf(fp,"\tTimestamp: %s", ctime(&commandTime));
fclose(fp);
sendZoeResponse(msg1);
IPC_freeByteArray(callData);
}
static void msgHandler (MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData)
{
FORMATTER_PTR formatter;
carmen_test_ipc_message msg;
formatter = IPC_msgInstanceFormatter(msgRef);
IPC_unmarshallData(formatter, callData, &msg,
sizeof(carmen_test_ipc_message));
IPC_freeByteArray(callData);
#if (defined(__x86_64__))
fprintf(stderr, "Got message from client %ld : time %f\n",
(long)clientData, msg.timestamp);
#else
fprintf(stderr, "Got message from client %d : time %f\n",
(int)clientData, msg.timestamp);
#endif
}
msg.vector_angle = angle;
err = IPC_publishData(CARMEN_ROBOT_VECTOR_STATUS_NAME, &msg);
carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_STATUS_NAME);
}
static void
velocity_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)))
{
carmen_robot_velocity_message v;
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, &v,
sizeof(carmen_robot_velocity_message));
IPC_freeByteArray(callData);
carmen_test_ipc_return(err, "Could not unmarshall",
IPC_msgInstanceName(msgRef));
time_of_last_command = carmen_get_time_ms();
command_rv = v.rv;
command_tv = v.tv;
if (collision_avoidance)
{
if (use_laser)
command_tv = carmen_clamp(carmen_robot_laser_min_rear_velocity(),
command_tv,
carmen_warn("Loading parameters for robot [31;1m%s[0m using param file "
"[31;1m%s[0m\n", selected_robot, param_filename);
return 0;
}
static
int lookup_ipc_query(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)),
carmen_param_query_message *query)
{
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err = IPC_OK;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, query,
sizeof(carmen_param_query_message));
IPC_freeByteArray(callData);
carmen_test_ipc_return_int(err, "Could not unmarshall",
IPC_msgInstanceName(msgRef));
return lookup_parameter(query->module_name, query->variable_name);
}
static void
get_robot(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused)))
{
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err = IPC_OK;
carmen_param_query_message query;
void qc_odometry_odometry_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){
(void) clientData; //no warning
IPC_RETURN_TYPE err = IPC_OK;
prev_omsg = omsg;
err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &omsg, sizeof(omsg));
if (newImuMessageAvailable){
newImuMessageAvailable = false;
prev_vmsg = vmsg;
vmsg.timestamp_sec = omsg.timestamp_sec;
vmsg.timestamp_usec = omsg.timestamp_usec;
if (msgcounter > 2)
validData = true;
if (validData){
double dtv = omsg.timestamp_sec - prev_omsg.timestamp_sec + 1e-6 * (omsg.timestamp_usec - prev_omsg.timestamp_usec);
double dta = vmsg.timestamp_sec - prev_vmsg.timestamp_sec + 1e-6 * (vmsg.timestamp_usec - prev_vmsg.timestamp_usec);
vmsg.x = omsg.x;
vmsg.y = omsg.y;
vmsg.z = omsg.z;
vmsg.q0 = omsg.q0;
vmsg.q1 = omsg.q1;
vmsg.q2 = omsg.q2;
vmsg.q3 = omsg.q3;
vmsg.roll = omsg.roll;
vmsg.pitch = omsg.pitch;
vmsg.yaw = omsg.yaw;
Transformation3<double> delta;
Transformation3<double> p0(DVector3(prev_omsg.x, prev_omsg.y, prev_omsg.z), Quaternion<double>(prev_omsg.q0, prev_omsg.q1, prev_omsg.q2, prev_omsg.q3));
Transformation3<double> p1(DVector3(omsg.x, omsg.y, omsg.z), Quaternion<double>(omsg.q0, omsg.q1, omsg.q2, omsg.q3));
delta = p0.inv() * p1;
///velocity && acceleration based on omsg
if (dtv > 1e-7){
dtv = 1./dtv;
vmsg.vx = delta.translationVector.x() * dtv;
vmsg.vy = delta.translationVector.y() * dtv;
vmsg.vz = delta.translationVector.z() * dtv;
if (dta > 1e-7){
///take into account the different orientations of the velocities!
p0.translationVector = DVector3(0,0,0);
p0.rotationQuaternion = Quaternion<double>(prev_vmsg.q0, prev_vmsg.q1, prev_vmsg.q2, prev_vmsg.q3);
p0.translationVector = p0 * DVector3(prev_vmsg.vx, prev_vmsg.vy, prev_vmsg.vz);
p1.translationVector = DVector3(0,0,0);
p1.rotationQuaternion = Quaternion<double>(vmsg.q0, vmsg.q1, vmsg.q2, vmsg.q3);
p1.translationVector = p1 * DVector3(vmsg.vx, vmsg.vy, vmsg.vz);
delta = p0.inv() * p1;
dta = 1./dta;
vmsg.avx = delta.translationVector.x() * dta;
vmsg.avy = delta.translationVector.y() * dta;
vmsg.avz = delta.translationVector.z() * dta;
} else {
vmsg.avx = vmsg.avy = vmsg.avz = 0;
}
} else {
vmsg.avx = vmsg.avy = vmsg.avz = 0;
vmsg.vx = vmsg.vy = vmsg.vz = 0;
}
///acceleration based on imu data
///FIXME
///acceleration based on tan roll and tan pitch
Quaternion<double> q(imsg.q0, imsg.q1, imsg.q2, imsg.q3);
DVector3 an = q.toAngles();
vmsg.axtanp = tan(an.pitch()) * 9.81;
vmsg.aytanr = tan(an.roll()) * 9.81;
qc_odometry_publish_velocity_message(&vmsg);
}
}
IPC_freeByteArray(callData);
IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&omsg);
msgcounter++;
if (!(msgcounter%10))
std::cerr << "." << std::flush;
}
err = IPC_respondData(msgRef, CARMEN_DOT_ALL_DOORS_MSG_NAME, &msg);
carmen_test_ipc(err, "Could not respond",
CARMEN_DOT_ALL_DOORS_MSG_NAME);
}
static void dot_query_handler
(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
void *clientData __attribute__ ((unused))) {
FORMATTER_PTR formatter;
IPC_RETURN_TYPE err;
carmen_dot_query query;
formatter = IPC_msgInstanceFormatter(msgRef);
err = IPC_unmarshallData(formatter, callData, &query, sizeof(carmen_dot_query));
IPC_freeByteArray(callData);
carmen_test_ipc_return(err, "Could not unmarshall",
IPC_msgInstanceName(msgRef));
printf("--> received query %d\n", query.type);
switch(query.type) {
case CARMEN_DOT_PERSON:
respond_all_people_msg(msgRef);
break;
case CARMEN_DOT_TRASH:
respond_all_trash_msg(msgRef);
break;
case CARMEN_DOT_DOOR:
respond_all_doors_msg(msgRef);