Added frame_id and timestamp to tool_position message