void FiducialsNode::tag_cb(int id, double x, double y, double z, double twist,
double dx, double dy, double dz, int visible) {
if (!publish_markers)
return;
visualization_msgs::Marker marker = createMarker(fiducial_namespace, id);
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, 0);
marker.scale.x = dx / scale;
marker.scale.y = dy / scale;
marker.scale.z = dz / scale;
if( visible ) {
marker.color = tag_color;
} else {
marker.color = hidden_tag_color;
}
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// publish text(ID) version of marker
char str_id[12];
snprintf(str_id, 12, "%d", id);
marker.text = str_id;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.pose.position.z += (marker.scale.z/2.0) + 0.05; // draw text above marker
marker.color.r = marker.color.g = marker.color.b = 1.0; // white
marker.scale.x = marker.scale.y = marker.scale.z = 0.2;
marker.id = id + 10000;
marker.ns = fiducial_namespace + "_text";
marker_pub->publish(marker);
}
void
main ()
{
UBYTE keys;
UBYTE pos, old_pos = 0;
UBYTE note, old_note = 0;
UBYTE relative_octave = 0;
UBYTE absolute_octave;
UBYTE waveform = pulse_50;
UBYTE mode = 0;
UBYTE root = C;
SCALE scale[8];
font_t big_font, small_font;
font_init ();
big_font = font_load (font_ibm);
small_font = font_load (font_spect);
font_set (big_font);
printf (";; Boueux v%s\n", BOUEUX_VERSION);
INIT_SOUND;
MASTER_VOLUME = OFF;
update_waveform (waveform);
MASTER_VOLUME = HIGH;
build_scale_mode (scale, root, mode);
for (;;)
{
keys = joypad ();
pos = scale_position (keys);
if (pos)
{
note = scale[pos - 1] + relative_octave*OCTAVE_LEN;
/* Raise by perfect 4th */
if (PRESSED (B)) note += 5; /* a perfect fourth = 5 semitones */
/* Lower by semitone */
if (PRESSED (A)) note -= 1;
}
/* Change octave */
if (PRESSED (START))
{
relative_octave = !relative_octave;
printf ("\n;; rel octave +%d\n", relative_octave);
WAIT_KEY_UP (START);
}
if (PRESSED (SELECT))
{
/* Change mode */
if (PRESSED (RIGHT))
{
mode = (mode + 1) % NUM_MODES;
WAIT_KEY_UP (RIGHT);
build_scale_mode (scale, root, mode);
}
/* Change waveform */
if (PRESSED (LEFT))
{
WAIT_KEY_UP (LEFT);
waveform = (waveform + 1) % NUM_WAVEFORMS;
update_waveform (waveform);
}
/* Increment root note */
if (PRESSED (UP))
{
WAIT_KEY_UP (UP);
root = (root + 1) % OCTAVE_LEN;
build_scale_mode (scale, root, mode);
}
/* Decrement root note */
if (PRESSED (DOWN))
{
WAIT_KEY_UP (DOWN);
if (root == 0)
root = OCTAVE_LEN - 1;
else
root = (root - 1) % OCTAVE_LEN;
build_scale_mode (scale, root, mode);
}
continue;
}
if ((note != old_note) || (pos != old_pos))
{
if (pos) /* Note will be played */
{
CH1_VOL = HIGH;
CH2_VOL = HIGH;
play_note (note, waveform);
font_set (small_font);
printf (note_names[note % OCTAVE_LEN]);
absolute_octave = note/OCTAVE_LEN + 3;
printf ("%d", absolute_octave);
printf (" ");
font_set (big_font);
}
else /* Stop note */
{
CH1_VOL = OFF;
CH2_VOL = OFF;
printf (". ");
}
}
if (waveform == wawa) wawa_update();
old_note = note;
old_pos = pos;
}
}
void FiducialsNode::location_cb(int id, double x, double y, double z,
double bearing) {
ROS_INFO("location_announce:id=%d x=%f y=%f bearing=%f",
id, x, y, bearing * 180. / 3.1415926);
visualization_msgs::Marker marker = createMarker(position_namespace, id);
ros::Time now = marker.header.stamp;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, bearing);
marker.scale.x = 0.2 / scale;
marker.scale.y = 0.05 / scale;
marker.scale.z = 0.05 / scale;
marker.color = position_color;
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// TODO: subtract out odometry position, and publish transform from
// map to odom
double tf_x = marker.pose.position.x;
double tf_y = marker.pose.position.y;
double tf_yaw = bearing;
// publish a transform based on the position
if( use_odom ) {
// if we're using odometry, look up the odom transform and subtract it
// from our position so that we can publish a map->odom transform
// such that map->odom->base_link reports the correct position
std::string tf_err;
if( tf_buffer.canTransform(pose_frame, odom_frame, now,
ros::Duration(0.1), &tf_err ) ) {
// get odometry position from TF
tf2::Quaternion tf_quat;
tf_quat.setRPY(0.0, 0.0, tf_yaw);
tf2::Transform pose(tf_quat, tf2::Vector3(tf_x, tf_y, 0));
// look up camera transform if we can
if( last_camera_frame.length() > 0 ) {
if( tf_buffer.canTransform(pose_frame, last_camera_frame, now,
ros::Duration(0.1), &tf_err) ) {
geometry_msgs::TransformStamped camera_tf;
camera_tf = tf_buffer.lookupTransform(pose_frame,
last_camera_frame, now);
tf2::Transform camera = msg_to_tf(camera_tf);
pose = pose * camera.inverse();
} else {
ROS_ERROR("Cannot look up transform from %s to %s: %s",
pose_frame.c_str(), last_camera_frame.c_str(), tf_err.c_str());
}
}
geometry_msgs::TransformStamped odom;
odom = tf_buffer.lookupTransform(odom_frame, pose_frame, now);
tf2::Transform odom_tf = msg_to_tf(odom);
// M = C * O
// C^-1 * M = O
// C^-1 = O * M-1
tf2::Transform odom_correction = (odom_tf * pose.inverse()).inverse();
geometry_msgs::TransformStamped transform;
tf2::Vector3 odom_correction_v = odom_correction.getOrigin();
transform.transform.translation.x = odom_correction_v.getX();
transform.transform.translation.y = odom_correction_v.getY();
transform.transform.translation.z = odom_correction_v.getZ();
tf2::Quaternion odom_correction_q = odom_correction.getRotation();
transform.transform.rotation.x = odom_correction_q.getX();
transform.transform.rotation.y = odom_correction_q.getY();
transform.transform.rotation.z = odom_correction_q.getZ();
transform.transform.rotation.w = odom_correction_q.getW();
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = odom_frame;
//tf2::transformTF2ToMsg(odom_correction, transform, now, world_frame,
//odom_frame);
if (publish_tf)
tf_pub.sendTransform(transform);
} else {
ROS_ERROR("Can't look up base transform from %s to %s: %s",
pose_frame.c_str(),
odom_frame.c_str(),
tf_err.c_str());
}
} else {
// we're publishing absolute position
geometry_msgs::TransformStamped transform;
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = pose_frame;
transform.transform.translation.x = tf_x;
transform.transform.translation.y = tf_y;
transform.transform.translation.z = 0.0;
transform.transform.rotation = tf::createQuaternionMsgFromYaw(tf_yaw);
if (publish_tf)
tf_pub.sendTransform(transform);
}
}
